Commit 3021da0d authored by Pierre Fernbach's avatar Pierre Fernbach
Browse files

refactor contact-phase done

parent 282846ca
......@@ -19,10 +19,10 @@ struct ContactModelPlanarTpl : public serialization::Serializable<ContactModelPl
typedef _Scalar Scalar;
/// \brief Default constructor.
/// \brief Default constructor.
ContactModelPlanarTpl() : m_mu(-1.), m_ZMP_radius(-1.) {}
/// \brief Default constructor.
/// \brief Default constructor.
ContactModelPlanarTpl(const Scalar mu, const Scalar ZMP_radius) : m_mu(mu), m_ZMP_radius(ZMP_radius) {}
/// \brief Copy constructor
......@@ -52,7 +52,6 @@ struct ContactModelPlanarTpl : public serialization::Serializable<ContactModelPl
/// \brief Friction coefficient.
Scalar m_mu;
/// \brief ZMP radius.
Scalar m_ZMP_radius;
......
// Copyright (c) 2015-2018, CNRS
// Authors: Justin Carpentier <jcarpent@laas.fr>
#ifndef __multicontact_api_scenario_contact_patch_hpp__
#define __multicontact_api_scenario_contact_patch_hpp__
#include "multicontact-api/scenario/fwd.hpp"
#include "multicontact-api/geometry/linear-cone.hpp"
#include <pinocchio/spatial/se3.hpp>
#include "multicontact-api/serialization/archive.hpp"
#include "multicontact-api/serialization/spatial.hpp"
#include "multicontact-api/scenario/contact-model-planar.hpp"
#include <pinocchio/spatial/se3.hpp>
#include <iostream>
namespace multicontact_api {
namespace scenario {
......@@ -19,69 +13,49 @@ namespace scenario {
template <typename _Scalar>
struct ContactPatchTpl : public serialization::Serializable<ContactPatchTpl<_Scalar> > {
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
typedef _Scalar Scalar;
typedef pinocchio::SE3Tpl<Scalar, 0> SE3;
typedef geometry::WrenchConeTpl<Scalar> LinearWrenchCone;
typedef ContactModelPlanarTpl<Scalar> ContactModel;
/// \brief Default constructor.
ContactPatchTpl()
: m_placement(SE3::Identity()), m_active(false), m_contact_model_placement(SE3::Identity()), m_lwc(0) {}
: m_placement(SE3::Identity()), m_mu(-1.) {}
/// \brief Init contact patch from a given placement.
explicit ContactPatchTpl(const SE3& placement)
: m_placement(placement), m_active(false), m_contact_model_placement(SE3::Identity()), m_lwc(0) {}
: m_placement(placement), m_mu(-1.) {}
/// \brief Init contact patch from a given placement and a friction coefficient
ContactPatchTpl(const SE3& placement, const Scalar mu)
: m_placement(placement), m_mu(mu) {}
/// \brief Copy constructor
ContactPatchTpl(const ContactPatchTpl& other)
: m_placement(other.m_placement),
m_active(other.m_active),
m_contact_model_placement(other.m_contact_model_placement),
m_oMcm(other.m_oMcm),
m_contact_model(other.m_contact_model),
m_lwc(other.m_lwc) {}
// ContactPatchTpl & operator=(const ContactPatchTpl & other)
// {
// std::cout << "Copy op" << std::endl;
// }
m_mu(other.m_mu) {}
const SE3& placement() const { return m_placement; }
SE3& placement() { return m_placement; }
bool active() const { return m_active; }
bool& active() { return m_active; }
const SE3& contactModelPlacement() const { return m_contact_model_placement; }
SE3& contactModelPlacement() { return m_contact_model_placement; }
const SE3& worldContactModelPlacement() const { return m_oMcm; }
SE3& worldContactModelPlacement() { return m_oMcm; }
const ContactModel& contactModel() const { return m_contact_model; }
ContactModel& contactModel() { return m_contact_model; }
const Scalar& friction() const { return m_mu; }
Scalar& friction() { return m_mu; }
const LinearWrenchCone& lwc() const { return m_lwc; }
LinearWrenchCone& lwc() { return m_lwc; }
template <typename S2>
bool operator==(const ContactPatchTpl<S2>& other) const {
return m_placement == other.m_placement && m_active == other.m_active &&
m_contact_model_placement == other.m_contact_model_placement && m_contact_model == other.m_contact_model &&
m_lwc == other.m_lwc;
return m_placement == other.m_placement && m_mu == other.m_mu;
}
template <typename S2>
bool operator!=(const ContactPatchTpl<S2>& other) const {
return !(*this != other);
return !(*this == other);
}
void disp(std::ostream& os) const {
os << "placement:\n"
os <<"Placement:\n"
<< m_placement << std::endl
<< "contact model placement:\n"
<< m_contact_model_placement << std::endl
<< "active: " << (m_active ? "True" : "False") << std::endl;
<<"Friction coefficient : "<<m_mu<<std::endl;
}
template <typename S2>
......@@ -90,48 +64,35 @@ struct ContactPatchTpl : public serialization::Serializable<ContactPatchTpl<_Sca
return os;
}
protected:
/// \brief Placement of the contact patch
SE3 m_placement;
/// \brief Is the contact patch active?
bool m_active;
// Relative to contact model
/// \brief Placement of the contact model w.r.t the contact patch
SE3 m_contact_model_placement;
/// \brief Placement of the contact model w.r.t the world
SE3 m_oMcm;
/// \brief Contact model (Planar,Bilateral)
ContactModel m_contact_model;
LinearWrenchCone m_lwc;
private:
// Serialization of the class
friend class boost::serialization::access;
template <class Archive>
void save(Archive& ar, const unsigned int /*version*/) const {
ar& boost::serialization::make_nvp("placement", m_placement);
ar& boost::serialization::make_nvp("active", m_active);
ar& boost::serialization::make_nvp("contact_model_placement", m_contact_model_placement);
ar& boost::serialization::make_nvp("contact_model", m_contact_model);
ar& boost::serialization::make_nvp("lwc", m_lwc);
}
template <class Archive>
void load(Archive& ar, const unsigned int /*version*/) {
ar >> boost::serialization::make_nvp("placement", m_placement);
ar >> boost::serialization::make_nvp("active", m_active);
ar >> boost::serialization::make_nvp("contact_model_placement", m_contact_model_placement);
ar >> boost::serialization::make_nvp("contact_model", m_contact_model);
ar >> boost::serialization::make_nvp("lwc", m_lwc);
}
protected:
/// \brief Placement of the contact patch
SE3 m_placement;
/// \brief friction coefficient for this contact
Scalar m_mu;
private:
// Serialization of the class
friend class boost::serialization::access;
template <class Archive>
void save(Archive& ar, const unsigned int /*version*/) const {
ar& boost::serialization::make_nvp("placement", m_placement);
ar& boost::serialization::make_nvp("mu", m_mu);
}
template <class Archive>
void load(Archive& ar, const unsigned int /*version*/) {
ar >> boost::serialization::make_nvp("placement", m_placement);
ar >> boost::serialization::make_nvp("mu", m_mu);
}
BOOST_SERIALIZATION_SPLIT_MEMBER() // why is it required ? using only serialize() lead to compilation error, probably because of the SE3
}; // struct ContactPatchTpl
}//scenario
}//multicontact_api
BOOST_SERIALIZATION_SPLIT_MEMBER()
};
} // namespace scenario
} // namespace multicontact_api
#endif // ifndef __multicontact_api_scenario_contact_patch_hpp__
#endif // __multicontact_api_scenario_contact_patch_hpp__
// Copyright (c) 2015-2018, CNRS
// Authors: Justin Carpentier <jcarpent@laas.fr>
#ifndef __multicontact_api_scenario_contact_patch_hpp__
#define __multicontact_api_scenario_contact_patch_hpp__
#include "multicontact-api/scenario/fwd.hpp"
#include "multicontact-api/geometry/linear-cone.hpp"
#include "multicontact-api/serialization/archive.hpp"
#include "multicontact-api/serialization/spatial.hpp"
#include "multicontact-api/scenario/contact-model-planar.hpp"
#include <pinocchio/spatial/se3.hpp>
#include <iostream>
namespace multicontact_api {
namespace scenario {
template <typename _Scalar>
struct ContactPatchTpl : public serialization::Serializable<ContactPatchTpl<_Scalar> > {
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
typedef _Scalar Scalar;
typedef pinocchio::SE3Tpl<Scalar, 0> SE3;
typedef geometry::WrenchConeTpl<Scalar> LinearWrenchCone;
typedef ContactModelPlanarTpl<Scalar> ContactModel;
/// \brief Default constructor.
ContactPatchTpl()
: m_placement(SE3::Identity()), m_active(false), m_contact_model_placement(SE3::Identity()), m_lwc(0) {}
/// \brief Init contact patch from a given placement.
explicit ContactPatchTpl(const SE3& placement)
: m_placement(placement), m_active(false), m_contact_model_placement(SE3::Identity()), m_lwc(0) {}
/// \brief Copy constructor
ContactPatchTpl(const ContactPatchTpl& other)
: m_placement(other.m_placement),
m_active(other.m_active),
m_contact_model_placement(other.m_contact_model_placement),
m_oMcm(other.m_oMcm),
m_contact_model(other.m_contact_model),
m_lwc(other.m_lwc) {}
// ContactPatchTpl & operator=(const ContactPatchTpl & other)
// {
// std::cout << "Copy op" << std::endl;
// }
const SE3& placement() const { return m_placement; }
SE3& placement() { return m_placement; }
bool active() const { return m_active; }
bool& active() { return m_active; }
const SE3& contactModelPlacement() const { return m_contact_model_placement; }
SE3& contactModelPlacement() { return m_contact_model_placement; }
const SE3& worldContactModelPlacement() const { return m_oMcm; }
SE3& worldContactModelPlacement() { return m_oMcm; }
const ContactModel& contactModel() const { return m_contact_model; }
ContactModel& contactModel() { return m_contact_model; }
const LinearWrenchCone& lwc() const { return m_lwc; }
LinearWrenchCone& lwc() { return m_lwc; }
template <typename S2>
bool operator==(const ContactPatchTpl<S2>& other) const {
return m_placement == other.m_placement && m_active == other.m_active &&
m_contact_model_placement == other.m_contact_model_placement && m_contact_model == other.m_contact_model &&
m_lwc == other.m_lwc;
}
template <typename S2>
bool operator!=(const ContactPatchTpl<S2>& other) const {
return !(*this != other);
}
void disp(std::ostream& os) const {
os << "placement:\n"
<< m_placement << std::endl
<< "contact model placement:\n"
<< m_contact_model_placement << std::endl
<< "active: " << (m_active ? "True" : "False") << std::endl;
}
template <typename S2>
friend std::ostream& operator<<(std::ostream& os, const ContactPatchTpl<S2>& cp) {
cp.disp(os);
return os;
}
protected:
/// \brief Placement of the contact patch
SE3 m_placement;
/// \brief Is the contact patch active?
bool m_active;
// Relative to contact model
/// \brief Placement of the contact model w.r.t the contact patch
SE3 m_contact_model_placement;
/// \brief Placement of the contact model w.r.t the world
SE3 m_oMcm;
/// \brief Contact model (Planar,Bilateral)
ContactModel m_contact_model;
LinearWrenchCone m_lwc;
private:
// Serialization of the class
friend class boost::serialization::access;
template <class Archive>
void save(Archive& ar, const unsigned int /*version*/) const {
ar& boost::serialization::make_nvp("placement", m_placement);
ar& boost::serialization::make_nvp("active", m_active);
ar& boost::serialization::make_nvp("contact_model_placement", m_contact_model_placement);
ar& boost::serialization::make_nvp("contact_model", m_contact_model);
ar& boost::serialization::make_nvp("lwc", m_lwc);
}
template <class Archive>
void load(Archive& ar, const unsigned int /*version*/) {
ar >> boost::serialization::make_nvp("placement", m_placement);
ar >> boost::serialization::make_nvp("active", m_active);
ar >> boost::serialization::make_nvp("contact_model_placement", m_contact_model_placement);
ar >> boost::serialization::make_nvp("contact_model", m_contact_model);
ar >> boost::serialization::make_nvp("lwc", m_lwc);
}
BOOST_SERIALIZATION_SPLIT_MEMBER()
};
} // namespace scenario
} // namespace multicontact_api
#endif // ifndef __multicontact_api_scenario_contact_patch_hpp__
......@@ -15,13 +15,11 @@ namespace scenario{
template<typename _Scalar> struct ContactPatchTpl;
typedef ContactPatchTpl<double> ContactPatch;
template<typename _Scalar, int _dim> struct ContactPhaseTpl;
typedef ContactPhaseTpl<double,4> ContactPhase4;
template<class _ContactPhase> struct ContactSequenceTpl;
typedef ContactSequenceTpl<ContactPhase4> ContactSequence4;
//typedef ContactSequenceTpl<ContactPhaseHumanoid> ContactSequenceHumanoid;
template<typename _Scalar> struct ContactPhaseTpl;
typedef ContactPhaseTpl<double> ContactPhase;
template<typename _Scalar> struct ContactSequenceTpl;
typedef ContactSequenceTpl<double> ContactSequence;
template<class SOC> struct ContactConstraintSOC;
typedef ContactConstraintSOC<geometry::SOC6d> ContactConstraintSOC6;
......@@ -32,16 +30,6 @@ namespace scenario{
template<typename Scalar> struct ContactConstraintPlanarTpl;
typedef ContactConstraintPlanarTpl<double> ContactConstraintPlanar;
enum HumanoidPhaseType
{
SINGLE_SUPPORT,
DOUBLE_SUPPORT,
TRIPLE_SUPPORT,
QUADRUPLE_SUPPORT,
NO_SUPPORT,
HUMANOID_PHASE_UNDEFINED
};
enum ConicType { CONIC_SOWC, CONIC_DOUBLE_DESCRIPTION, CONIC_UNDEFINED };
}
......
// Copyright (c) 2015-2018, CNRS
// Authors: Justin Carpentier <jcarpent@laas.fr>
//
#ifndef __multicontact_api_scenario_ms_interval_hpp__
#define __multicontact_api_scenario_ms_interval_hpp__
#include "multicontact-api/scenario/fwd.hpp"
#include "multicontact-api/serialization/archive.hpp"
#include "multicontact-api/serialization/eigen-matrix.hpp"
#include <pinocchio/container/aligned-vector.hpp>
namespace multicontact_api {
namespace scenario {
template <typename _TimeVector, typename _StateVector, typename _ControlVector>
struct MSIntervalDataTpl
: public serialization::Serializable<MSIntervalDataTpl<_TimeVector, _StateVector, _ControlVector> > {
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
typedef _TimeVector TimeVector;
typedef _StateVector StateVector;
typedef pinocchio::container::aligned_vector<StateVector> StateVectorTrajectory;
typedef _ControlVector ControlVector;
typedef pinocchio::container::aligned_vector<ControlVector> ControlVectorTrajectory;
/// \brief Default constructor
MSIntervalDataTpl() {}
/// \brief Copy constructor
MSIntervalDataTpl(const MSIntervalDataTpl& other)
: m_time_trajectory(other.m_time_trajectory),
m_state_trajectory(other.m_state_trajectory),
m_dot_state_trajectory(other.m_dot_state_trajectory),
m_control_trajectory(other.m_control_trajectory) {}
/// \Returns a reference of the time trajectory
const TimeVector& time_trajectory() const { return m_time_trajectory; }
/// \Returns a reference of the time trajectory
TimeVector& time_trajectory() { return m_time_trajectory; }
const StateVectorTrajectory& state_trajectory() const { return m_state_trajectory; }
StateVectorTrajectory& state_trajectory() { return m_state_trajectory; }
const StateVectorTrajectory& dot_state_trajectory() const { return m_dot_state_trajectory; }
StateVectorTrajectory& dot_state_trajectory() { return m_dot_state_trajectory; }
const ControlVectorTrajectory& control_trajectory() const { return m_control_trajectory; }
ControlVectorTrajectory& control_trajectory() { return m_control_trajectory; }
bool operator==(const MSIntervalDataTpl& other) const {
return m_time_trajectory == other.m_time_trajectory && m_state_trajectory == other.m_state_trajectory &&
m_dot_state_trajectory == other.m_dot_state_trajectory &&
m_control_trajectory == other.m_control_trajectory;
}
bool operator!=(const MSIntervalDataTpl& other) const { return !(*this == other); }
protected:
/// \brief Timeline of the shooting interval.
TimeVector m_time_trajectory;
/// \brief State and derivate of the state trajectories over the shooting intervals.
StateVectorTrajectory m_state_trajectory, m_dot_state_trajectory;
/// \brief Control trajectory over the shooting interval
ControlVectorTrajectory m_control_trajectory;
private:
// Serialization of the class
friend class boost::serialization::access;
template <class Archive>
void save(Archive& ar, const unsigned int /*version*/) const {
ar& boost::serialization::make_nvp("time_trajectory", m_time_trajectory);
ar& boost::serialization::make_nvp("state_trajectory", m_state_trajectory);
ar& boost::serialization::make_nvp("dot_state_trajectory", m_dot_state_trajectory);
ar& boost::serialization::make_nvp("control_trajectory", m_control_trajectory);
}
template <class Archive>
void load(Archive& ar, const unsigned int /*version*/) {
ar >> boost::serialization::make_nvp("time_trajectory", m_time_trajectory);
ar >> boost::serialization::make_nvp("state_trajectory", m_state_trajectory);
ar >> boost::serialization::make_nvp("dot_state_trajectory", m_dot_state_trajectory);
ar >> boost::serialization::make_nvp("control_trajectory", m_control_trajectory);
}
BOOST_SERIALIZATION_SPLIT_MEMBER()
};
} // namespace scenario
} // namespace multicontact_api
#endif // ifndef __multicontact_api_scenario_ms_interval_hpp__
......@@ -7,9 +7,10 @@
#include <boost/test/unit_test.hpp>
#include <boost/utility/binary.hpp>
#include "multicontact-api/scenario/contact-phase.hpp"
#include "multicontact-api/scenario/contact-sequence.hpp"
#include "multicontact-api/scenario/contact-model-planar.hpp"
#include "multicontact-api/scenario/contact-patch.hpp"
//#include "multicontact-api/scenario/contact-phase.hpp"
//#include "multicontact-api/scenario/contact-sequence.hpp"
using namespace multicontact_api::scenario;
......@@ -27,6 +28,8 @@ struct ATpl {
};
typedef ATpl<double> Ad;
typedef pinocchio::SE3Tpl<double> SE3;
BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)
......@@ -41,53 +44,95 @@ BOOST_AUTO_TEST_CASE(contact_model) {
}
BOOST_AUTO_TEST_CASE(contact_patch) {
// check default constructor :
ContactPatch cp;
cp.contactModel().m_mu = 0.3;
cp.contactModel().m_ZMP_radius = 0.01;
cp.placement().setRandom();
cp.contactModelPlacement().setRandom();
cp.worldContactModelPlacement().setRandom();
ContactPatch cp2(cp);
ContactPatch cp3 = cp2;
BOOST_CHECK(cp == cp);
BOOST_CHECK(cp == cp2);
Ad a1;
Ad a2(a1);
}
BOOST_AUTO_TEST_CASE(contact_phase)
{
ContactPhase4 cp, cp_test;
for(ContactPhase4::ContactPatchMap::iterator it = cp.contact_patches().begin();
it != cp.contact_patches().end(); ++it)
{
it->second.contactModel().m_mu = 0.3;
it->second.contactModel().m_ZMP_radius = 0.01;
it->second.placement().setRandom();
it->second.contactModelPlacement().setRandom();
it->second.worldContactModelPlacement().setRandom();
}
ContactPhase4 cp2(cp);
BOOST_CHECK(cp == cp);
BOOST_CHECK(cp == cp2);
// test serialization
cp.saveAsText("serialization_cp_test.test");
cp_test.loadFromText("serialization_cp_test.test");
remove("serialization_cp_test.test");
BOOST_CHECK(cp == cp_test);
BOOST_CHECK(cp.placement() == SE3::Identity());
BOOST_CHECK(cp.friction() == -1.);
SE3 p = SE3::Identity();
p.setRandom();
cp.placement() = p;
BOOST_CHECK(cp.placement() == p);
cp.friction() = 0.7;
BOOST_CHECK(cp.friction() == 0.7);
// constructor with placement :
p.setRandom();
ContactPatch cp1(p);
BOOST_CHECK(cp1.placement() == p);
BOOST_CHECK(cp1.friction() == -1.);
// constructor with placement and friction
p.setRandom();
ContactPatch cp2(p,0.9);
BOOST_CHECK(cp2.placement() == p);
BOOST_CHECK(cp2.friction() == 0.9);
// check comparison operator
BOOST_CHECK(cp1 != cp2);
ContactPatch cp3(p,0.9);
BOOST_CHECK(cp3 == cp2);
cp2.friction() = 0.1;
BOOST_CHECK(cp3 != cp2);
// copy constructor
ContactPatch cp4(cp3);
BOOST_CHECK(cp4 == cp3);
BOOST_CHECK(cp4.placement() == p);
BOOST_CHECK(cp4.friction() == 0.9);
cp4.placement() = SE3::Identity();
BOOST_CHECK(cp3 != cp4);
// serialization :
std::string fileName("fileTest");
cp3.saveAsText(fileName);
ContactPatch cp_from_text;
cp_from_text.loadFromText(fileName);
BOOST_CHECK(cp3 == cp_from_text);
cp3.saveAsXML(fileName,"ContactPatch");
ContactPatch cp_from_xml;
cp_from_xml.loadFromXML(fileName,"ContactPatch");
BOOST_CHECK(cp3 == cp_from_xml);
cp3.saveAsBinary(fileName);
ContactPatch cp_from_bin;
cp_from_bin.loadFromBinary(fileName);
BOOST_CHECK(cp3 == cp_from_bin);
}
BOOST_AUTO_TEST_CASE(contact_sequence)
{
ContactPhase4 cp;
ContactSequence4 cs(1);
ContactSequence4 cs_test(0);
cs.m_contact_phases[0] = cp;
// test serialization
cs.saveAsText("serialization_cs_test.test");
cs_test.loadFromText("serialization_cs_test.test");
remove("serialization_cp_test.test");
BOOST_CHECK(cs == cs_test);
}