Unverified Commit f7f0dda2 authored by Fernbach Pierre's avatar Fernbach Pierre Committed by GitHub
Browse files

Merge pull request #11 from pFernbach/topic/contact_model

Improve ContactModel
parents 73409534 aa9ad648
No preview for this file type
...@@ -15,7 +15,7 @@ SET(${PROJECT_NAME}_SERIALIZATION_HEADERS ...@@ -15,7 +15,7 @@ SET(${PROJECT_NAME}_SERIALIZATION_HEADERS
) )
SET(${PROJECT_NAME}_SCENARIO_HEADERS SET(${PROJECT_NAME}_SCENARIO_HEADERS
scenario/contact-model-planar.hpp scenario/contact-model.hpp
scenario/contact-patch.hpp scenario/contact-patch.hpp
scenario/contact-phase.hpp scenario/contact-phase.hpp
scenario/contact-sequence.hpp scenario/contact-sequence.hpp
...@@ -29,7 +29,7 @@ SET(${PROJECT_NAME}_PYTHON_HEADERS ...@@ -29,7 +29,7 @@ SET(${PROJECT_NAME}_PYTHON_HEADERS
bindings/python/geometry/linear-cone.hpp bindings/python/geometry/linear-cone.hpp
bindings/python/geometry/second-order-cone.hpp bindings/python/geometry/second-order-cone.hpp
bindings/python/serialization/archive.hpp bindings/python/serialization/archive.hpp
bindings/python/scenario/contact-model-planar.hpp bindings/python/scenario/contact-model.hpp
bindings/python/scenario/contact-patch.hpp bindings/python/scenario/contact-patch.hpp
bindings/python/scenario/contact-phase.hpp bindings/python/scenario/contact-phase.hpp
bindings/python/scenario/contact-sequence.hpp bindings/python/scenario/contact-sequence.hpp
......
// Copyright (c) 2015-2018, CNRS
// Authors: Justin Carpentier <jcarpent@laas.fr>
#ifndef __multicontact_api_python_scenario_contact_model_planar_hpp__
#define __multicontact_api_python_scenario_contact_model_planar_hpp__
#include <string>
#include "multicontact-api/scenario/contact-model-planar.hpp"
#include "multicontact-api/bindings/python/serialization/archive.hpp"
#include "multicontact-api/bindings/python/utils/printable.hpp"
namespace multicontact_api {
namespace python {
namespace bp = boost::python;
template <typename ContactModelPlanar>
struct ContactModelPlanarPythonVisitor
: public boost::python::def_visitor<ContactModelPlanarPythonVisitor<ContactModelPlanar> > {
typedef typename ContactModelPlanar::Scalar Scalar;
template <class PyClass>
void visit(PyClass& cl) const {
cl.def(bp::init<>())
.def(bp::init<Scalar, Scalar>(bp::args("mu", "ZMP_radius")))
.def(bp::init<ContactModelPlanar>(bp::args("other"), "Copy contructor."))
.def_readwrite("mu", &ContactModelPlanar::m_mu, "Friction coefficient.")
.def_readwrite("ZMP_radius", &ContactModelPlanar::m_ZMP_radius, "Radius of the ZMP region.")
.def(bp::self == bp::self)
.def(bp::self != bp::self)
.def("copy", &copy, "Returns a copy of *this.");
}
static void expose(const std::string& class_name) {
std::string doc = "Contact Model Planar";
bp::class_<ContactModelPlanar>(class_name.c_str(), doc.c_str(), bp::no_init)
.def(ContactModelPlanarPythonVisitor<ContactModelPlanar>())
.def(SerializableVisitor<ContactModelPlanar>())
.def(PrintableVisitor<ContactModelPlanar>());
}
private:
static ContactModelPlanar copy(const ContactModelPlanar& self) { return ContactModelPlanar(self); }
};
} // namespace python
} // namespace multicontact_api
#endif // ifndef __multicontact_api_python_scenario_contact_model_planar_hpp__
// Copyright (c) 2015-2018, CNRS
// Authors: Justin Carpentier <jcarpent@laas.fr>
#ifndef __multicontact_api_python_scenario_contact_model_planar_hpp__
#define __multicontact_api_python_scenario_contact_model_planar_hpp__
#include <string>
#include "multicontact-api/scenario/fwd.hpp"
#include "multicontact-api/scenario/contact-model.hpp"
#include "multicontact-api/bindings/python/serialization/archive.hpp"
#include "multicontact-api/bindings/python/utils/printable.hpp"
#include <eigenpy/eigenpy.hpp>
namespace multicontact_api {
namespace python {
namespace bp = boost::python;
template <typename ContactModel>
struct ContactModelPythonVisitor : public boost::python::def_visitor<ContactModelPythonVisitor<ContactModel> > {
typedef typename ContactModel::Scalar Scalar;
typedef scenario::ContactType ContactType;
typedef typename ContactModel::Matrix3X Matrix3X;
typedef typename ContactModel::Matrix6X Matrix6X;
template <class PyClass>
void visit(PyClass& cl) const {
cl.def(bp::init<>())
.def(bp::init<Scalar>(bp::args("mu")))
.def(bp::init<Scalar, ContactType>(bp::args("mu", "contact_type")))
.def(bp::init<ContactModel>(bp::args("other"), "Copy contructor."))
.def_readwrite("mu", &ContactModel::m_mu, "Friction coefficient.")
.def_readwrite("contact_type", &ContactModel::m_contact_type, "Enum that define the type of contact.")
.add_property("num_contact_points", &getNumContact, &setNumContact,
"The number of contact points used to model this contact. \n"
"Changing this value will clear the contact_points_positions matrix")
.add_property("contact_points_positions", &getContactPositions, &setContactPositions,
"3xnum_contact_points matrix defining the contact points positions in the frame of the contact "
"placement. \n"
"num_contact_points is automatically updated to the number of columns of this matrix.")
.def("generatorMatrix", &ContactModel::generatorMatrix,
"generatorMatrix Return a 6x(num_contact_points*3) matrix"
"containing the generator used to compute contact forces.")
.def(bp::self == bp::self)
.def(bp::self != bp::self)
.def("copy", &copy, "Returns a copy of *this.");
}
static void expose(const std::string& class_name) {
std::string doc = "Contact Model";
bp::class_<ContactModel>(class_name.c_str(), doc.c_str(), bp::no_init)
.def(ContactModelPythonVisitor<ContactModel>())
.def(SerializableVisitor<ContactModel>())
.def(PrintableVisitor<ContactModel>());
ENABLE_SPECIFIC_MATRIX_TYPE(Matrix3X);
ENABLE_SPECIFIC_MATRIX_TYPE(Matrix6X);
}
private:
static ContactModel copy(const ContactModel& self) { return ContactModel(self); }
// define setter and getter
static size_t getNumContact(ContactModel& self) { return self.num_contact_points(); }
static void setNumContact(ContactModel& self, const size_t num) { self.num_contact_points(num); }
static Matrix3X getContactPositions(ContactModel& self) { return self.contact_points_positions(); }
static void setContactPositions(ContactModel& self, const Matrix3X& pos) { self.contact_points_positions(pos); }
};
} // namespace python
} // namespace multicontact_api
#endif // ifndef __multicontact_api_python_scenario_contact_model_planar_hpp__
...@@ -34,6 +34,7 @@ struct ContactPatchPythonVisitor : public boost::python::def_visitor<ContactPatc ...@@ -34,6 +34,7 @@ struct ContactPatchPythonVisitor : public boost::python::def_visitor<ContactPatc
"Placement of the patch represented as a pinocchio SE3 object.") "Placement of the patch represented as a pinocchio SE3 object.")
.add_property("friction", &getFriction, &setFriction, .add_property("friction", &getFriction, &setFriction,
"Friction coefficient between the robot and the environment for this contact.") "Friction coefficient between the robot and the environment for this contact.")
.def_readwrite("contact_model", &ContactPatch::m_contact_model, "The contact model defining this contact.")
.def(bp::self == bp::self) .def(bp::self == bp::self)
.def(bp::self != bp::self) .def(bp::self != bp::self)
.def("copy", &copy, "Returns a copy of *this."); .def("copy", &copy, "Returns a copy of *this.");
......
...@@ -160,6 +160,8 @@ struct ContactSequencePythonVisitor : public bp::def_visitor<ContactSequencePyth ...@@ -160,6 +160,8 @@ struct ContactSequencePythonVisitor : public bp::def_visitor<ContactSequencePyth
.def("haveFriction", &CS::haveFriction, .def("haveFriction", &CS::haveFriction,
"check that all the contact patch used in the sequence have" "check that all the contact patch used in the sequence have"
"a friction coefficient initialized.") "a friction coefficient initialized.")
.def("haveContactModelDefined", &CS::haveContactModelDefined,
"haveContactModelDefined check that all the contact patch have a contact_model defined")
.def("haveZMPtrajectories", &CS::haveZMPtrajectories, .def("haveZMPtrajectories", &CS::haveZMPtrajectories,
"check that all the contact phases have a ZMP trajectory.") "check that all the contact phases have a ZMP trajectory.")
.def("getAllEffectorsInContact", &getAllEffectorsInContactAsList, .def("getAllEffectorsInContact", &getAllEffectorsInContactAsList,
...@@ -260,7 +262,6 @@ struct ContactSequencePythonVisitor : public bp::def_visitor<ContactSequencePyth ...@@ -260,7 +262,6 @@ struct ContactSequencePythonVisitor : public bp::def_visitor<ContactSequencePyth
static bp::list getAllEffectorsInContactAsList(CS& self) { static bp::list getAllEffectorsInContactAsList(CS& self) {
return toPythonList<std::string>(self.getAllEffectorsInContact()); return toPythonList<std::string>(self.getAllEffectorsInContact());
} }
}; };
} // namespace python } // namespace python
} // namespace multicontact_api } // namespace multicontact_api
......
...@@ -13,22 +13,20 @@ namespace bp = boost::python; ...@@ -13,22 +13,20 @@ namespace bp = boost::python;
template <typename Derived> template <typename Derived>
struct cs_pickle_suite : bp::pickle_suite { struct cs_pickle_suite : bp::pickle_suite {
static bp::object getstate(const Derived& cs) {
std::ostringstream os;
boost::archive::text_oarchive oa(os);
oa << cs;
return bp::str(os.str());
}
static bp::object getstate (const Derived& cs) { static void setstate(Derived& cs, bp::object entries) {
std::ostringstream os; bp::str s = bp::extract<bp::str>(entries)();
boost::archive::text_oarchive oa(os); std::string st = bp::extract<std::string>(s)();
oa << cs; std::istringstream is(st);
return bp::str(os.str()); boost::archive::text_iarchive ia(is);
} ia >> cs;
}
static void
setstate(Derived& cs, bp::object entries) {
bp::str s = bp::extract<bp::str> (entries)();
std::string st = bp::extract<std::string> (s)();
std::istringstream is (st);
boost::archive::text_iarchive ia (is);
ia >> cs;
}
}; };
template <typename Derived> template <typename Derived>
......
// Copyright (c) 2015-2018, CNRS
// Authors: Justin Carpentier <jcarpent@laas.fr>
#ifndef __multicontact_api_scenario_contact_model_planar_hpp__
#define __multicontact_api_scenario_contact_model_planar_hpp__
#include "multicontact-api/scenario/fwd.hpp"
#include "multicontact-api/serialization/archive.hpp"
#include <iostream>
#include <Eigen/Dense>
namespace multicontact_api {
namespace scenario {
template <typename _Scalar>
struct ContactModelPlanarTpl : public serialization::Serializable<ContactModelPlanarTpl<_Scalar> > {
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
typedef _Scalar Scalar;
/// \brief Default constructor.
ContactModelPlanarTpl() : m_mu(-1.), m_ZMP_radius(-1.) {}
/// \brief Default constructor.
ContactModelPlanarTpl(const Scalar mu, const Scalar ZMP_radius) : m_mu(mu), m_ZMP_radius(ZMP_radius) {}
/// \brief Copy constructor
template <typename S2>
explicit ContactModelPlanarTpl(const ContactModelPlanarTpl<S2>& other)
: m_mu(other.mu), m_ZMP_radius(other.ZMP_radius) {}
template <typename S2>
bool operator==(const ContactModelPlanarTpl<S2>& other) const {
return m_mu == other.m_mu && m_ZMP_radius == other.m_ZMP_radius;
}
template <typename S2>
bool operator!=(const ContactModelPlanarTpl<S2>& other) const {
return !(*this == other);
}
void disp(std::ostream& os) const {
os << "mu: " << m_mu << std::endl << "ZMP radius: " << m_ZMP_radius << std::endl;
}
template <typename S2>
friend std::ostream& operator<<(std::ostream& os, const ContactModelPlanarTpl<S2>& cp) {
cp.disp(os);
return os;
}
/// \brief Friction coefficient.
Scalar m_mu;
/// \brief ZMP radius.
Scalar m_ZMP_radius;
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("mu", m_mu);
ar& boost::serialization::make_nvp("ZMP_radius", m_ZMP_radius);
}
template <class Archive>
void load(Archive& ar, const unsigned int /*version*/) {
ar >> boost::serialization::make_nvp("mu", m_mu);
ar >> boost::serialization::make_nvp("ZMP_radius", m_ZMP_radius);
}
BOOST_SERIALIZATION_SPLIT_MEMBER()
};
} // namespace scenario
} // namespace multicontact_api
#endif // ifndef __multicontact_api_scenario_contact_model_planar_hpp__
// Copyright (c) 2015-2018, CNRS
// Authors: Justin Carpentier <jcarpent@laas.fr>
#ifndef __multicontact_api_scenario_contact_model_planar_hpp__
#define __multicontact_api_scenario_contact_model_planar_hpp__
#include "multicontact-api/scenario/fwd.hpp"
#include "multicontact-api/serialization/archive.hpp"
#include "multicontact-api/serialization/eigen-matrix.hpp"
#include <iostream>
#include <Eigen/Dense>
#include <pinocchio/spatial/skew.hpp>
namespace multicontact_api {
namespace scenario {
template <typename _Scalar>
struct ContactModelTpl : public serialization::Serializable<ContactModelTpl<_Scalar> > {
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
typedef _Scalar Scalar;
typedef Eigen::Matrix<Scalar, 3, 3> Matrix3;
typedef Eigen::Matrix<Scalar, 3, Eigen::Dynamic> Matrix3X;
typedef Eigen::Matrix<Scalar, 6, Eigen::Dynamic> Matrix6X;
/// \brief Default constructor.
ContactModelTpl()
: m_mu(-1.),
m_contact_type(ContactType::CONTACT_UNDEFINED),
m_num_contact_points(1),
m_contact_points_positions(Matrix3X::Zero(3, 1)) {}
/// \brief Constructor with friction
ContactModelTpl(const Scalar mu)
: m_mu(mu),
m_contact_type(ContactType::CONTACT_UNDEFINED),
m_num_contact_points(1),
m_contact_points_positions(Matrix3X::Zero(3, 1)) {}
/// \brief Full constructor
ContactModelTpl(const Scalar mu, const ContactType contact_type)
: m_mu(mu),
m_contact_type(contact_type),
m_num_contact_points(1),
m_contact_points_positions(Matrix3X::Zero(3, 1)) {}
/// \brief Copy constructor
template <typename S2>
explicit ContactModelTpl(const ContactModelTpl<S2>& other)
: m_mu(other.mu),
m_contact_type(other.m_contact_type),
m_contact_points_positions(other.m_contact_points_positions),
m_num_contact_points(other.m_num_contact_points) {}
template <typename S2>
bool operator==(const ContactModelTpl<S2>& other) const {
return m_mu == other.m_mu && m_contact_type == other.m_contact_type &&
m_num_contact_points == other.m_num_contact_points &&
m_contact_points_positions == other.m_contact_points_positions;
}
template <typename S2>
bool operator!=(const ContactModelTpl<S2>& other) const {
return !(*this == other);
}
void disp(std::ostream& os) const {
os << "ContactType: " << m_contact_type << ", mu: " << m_mu << std::endl
<< "Number of contact points: " << m_num_contact_points << ", positions: " << std::endl
<< m_contact_points_positions << std::endl;
}
template <typename S2>
friend std::ostream& operator<<(std::ostream& os, const ContactModelTpl<S2>& cp) {
cp.disp(os);
return os;
}
size_t num_contact_points() const { return m_num_contact_points; }
void num_contact_points(const size_t num) {
m_num_contact_points = num;
m_contact_points_positions = Matrix3X::Zero(3, num);
}
Matrix3X contact_points_positions() const { return m_contact_points_positions; }
void contact_points_positions(const Matrix3X& positions) {
m_contact_points_positions = positions;
m_num_contact_points = positions.cols();
}
/**
* @brief generatorMatrix Return a 6x(num_contact_points*3) matrix
* containing the generator used to compute contact forces.
* @return
*/
Matrix6X generatorMatrix() const {
Matrix6X gen = Matrix6X::Zero(6, m_num_contact_points * 3);
for(size_t i=0; i<m_num_contact_points; i++)
{
gen.block(0, i*3, 3, 3) = Matrix3::Identity();
gen.block(3, i*3, 3, 3) = pinocchio::skew(m_contact_points_positions.col(i));
}
return gen;
}
/// \brief Friction coefficient.
Scalar m_mu;
/// \brief ZMP radius.
ContactType m_contact_type;
private:
/// \brief The number of contact points used to model this contact
size_t m_num_contact_points;
/// \brief 3xnum_contact_points matrix defining the contact points positions in the frame of the contact placement
Matrix3X m_contact_points_positions;
// 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("mu", m_mu);
ar& boost::serialization::make_nvp("contact_type", m_contact_type);
ar& boost::serialization::make_nvp("num_contact_points", m_num_contact_points);
ar& boost::serialization::make_nvp("contact_points_positions", m_contact_points_positions);
}
template <class Archive>
void load(Archive& ar, const unsigned int /*version*/) {
ar >> boost::serialization::make_nvp("mu", m_mu);
ar >> boost::serialization::make_nvp("contact_type", m_contact_type);
ar >> boost::serialization::make_nvp("num_contact_points", m_num_contact_points);
ar >> boost::serialization::make_nvp("contact_points_positions", m_contact_points_positions);
}
BOOST_SERIALIZATION_SPLIT_MEMBER()
};
} // namespace scenario
} // namespace multicontact_api
#endif // ifndef __multicontact_api_scenario_contact_model_planar_hpp__
...@@ -3,6 +3,7 @@ ...@@ -3,6 +3,7 @@
#define __multicontact_api_scenario_contact_patch_hpp__ #define __multicontact_api_scenario_contact_patch_hpp__
#include "multicontact-api/scenario/fwd.hpp" #include "multicontact-api/scenario/fwd.hpp"
#include "multicontact-api/scenario/contact-model.hpp"
#include <pinocchio/spatial/se3.hpp> #include <pinocchio/spatial/se3.hpp>
#include "multicontact-api/serialization/archive.hpp" #include "multicontact-api/serialization/archive.hpp"
#include "multicontact-api/serialization/spatial.hpp" #include "multicontact-api/serialization/spatial.hpp"
...@@ -15,29 +16,31 @@ struct ContactPatchTpl : public serialization::Serializable<ContactPatchTpl<_Sca ...@@ -15,29 +16,31 @@ struct ContactPatchTpl : public serialization::Serializable<ContactPatchTpl<_Sca
EIGEN_MAKE_ALIGNED_OPERATOR_NEW EIGEN_MAKE_ALIGNED_OPERATOR_NEW
typedef _Scalar Scalar; typedef _Scalar Scalar;
typedef ContactModelTpl<_Scalar> ContactModel;
typedef pinocchio::SE3Tpl<Scalar, 0> SE3; typedef pinocchio::SE3Tpl<Scalar, 0> SE3;
/// \brief Default constructor. /// \brief Default constructor.
ContactPatchTpl() : m_placement(SE3::Identity()), m_mu(-1.) {} ContactPatchTpl() : m_contact_model(), m_placement(SE3::Identity()) {}
/// \brief Init contact patch from a given placement. /// \brief Init contact patch from a given placement.
explicit ContactPatchTpl(const SE3& placement) : m_placement(placement), m_mu(-1.) {} explicit ContactPatchTpl(const SE3& placement) : m_contact_model(), m_placement(placement) {}
/// \brief Init contact patch from a given placement and a friction coefficient /// \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) {} ContactPatchTpl(const SE3& placement, const Scalar mu) : m_contact_model(mu), m_placement(placement) {}
/// \brief Copy constructor /// \brief Copy constructor
ContactPatchTpl(const ContactPatchTpl& other) : m_placement(other.m_placement), m_mu(other.m_mu) {} ContactPatchTpl(const ContactPatchTpl& other)
: m_contact_model(other.m_contact_model), m_placement(other.m_placement) {}
const SE3& placement() const { return m_placement; } const SE3& placement() const { return m_placement; }
SE3& placement() { return m_placement; } SE3& placement() { return m_placement; }
const Scalar& friction() const { return m_mu; } const Scalar& friction() const { return m_contact_model.m_mu; }
Scalar& friction() { return m_mu; } Scalar& friction() { return m_contact_model.m_mu; }
template <typename S2> template <typename S2>
bool operator==(const ContactPatchTpl<S2>& other) const { bool operator==(const ContactPatchTpl<S2>& other) const {
return m_placement == other.m_placement && m_mu == other.m_mu; return m_placement == other.m_placement && m_contact_model == other.m_contact_model;
} }
template <typename S2> template <typename S2>
...@@ -46,7 +49,7 @@ struct ContactPatchTpl : public serialization::Serializable<ContactPatchTpl<_Sca ...@@ -46,7 +49,7 @@ struct ContactPatchTpl : public serialization::Serializable<ContactPatchTpl<_Sca
} }
void disp(std::ostream& os) const { void disp(std::ostream& os) const {
os << "Placement:\n" << m_placement << std::endl << "Friction coefficient : " << m_mu << std::endl; os << "Placement:\n" << m_placement << std::endl << "ContactModel : " << m_contact_model << std::endl;
} }
template <typename S2> template <typename S2>
...@@ -55,11 +58,12 @@ struct ContactPatchTpl : public serialization::Serializable<ContactPatchTpl<_Sca ...@@ -55,11 +58,12 @@ struct ContactPatchTpl : public serialization::Serializable<ContactPatchTpl<_Sca
return os; return os;
} }
/// \brief Contact model of this contact
ContactModel m_contact_model;
protected: protected:
/// \brief Placement of the contact patch /// \brief Placement of the contact patch
SE3 m_placement; SE3 m_placement;
/// \brief friction coefficient for this contact
Scalar m_mu;
private: private:
// Serialization of the class // Serialization of the class
...@@ -68,13 +72,13 @@ struct ContactPatchTpl : public serialization::Serializable<ContactPatchTpl<_Sca ...@@ -68,13 +72,13 @@ struct ContactPatchTpl : public serialization::Serializable<ContactPatchTpl<_Sca
template <class Archive> template <class Archive>
void save(Archive& ar, const unsigned int /*version*/) const { void save(Archive& ar, const unsigned int /*version*/) const {
ar& boost::serialization::make_nvp("placement", m_placement); ar& boost::serialization::make_nvp("placement", m_placement);
ar& boost::serialization::make_nvp("mu", m_mu); ar& boost::serialization::make_nvp("contact_model", m_contact_model);
} }
template <class Archive> template <class Archive>
void load(Archive& ar, const unsigned int /*version*/) { void load(Archive& ar, const unsigned int /*version*/) {
ar >> boost::serialization::make_nvp("placement", m_placement); ar >> boost::serialization::make_nvp("placement", m_placement);