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
)
SET(${PROJECT_NAME}_SCENARIO_HEADERS
scenario/contact-model-planar.hpp
scenario/contact-model.hpp
scenario/contact-patch.hpp
scenario/contact-phase.hpp
scenario/contact-sequence.hpp
......@@ -29,7 +29,7 @@ SET(${PROJECT_NAME}_PYTHON_HEADERS
bindings/python/geometry/linear-cone.hpp
bindings/python/geometry/second-order-cone.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-phase.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
"Placement of the patch represented as a pinocchio SE3 object.")
.add_property("friction", &getFriction, &setFriction,
"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("copy", &copy, "Returns a copy of *this.");
......
......@@ -160,6 +160,8 @@ struct ContactSequencePythonVisitor : public bp::def_visitor<ContactSequencePyth
.def("haveFriction", &CS::haveFriction,
"check that all the contact patch used in the sequence have"
"a friction coefficient initialized.")
.def("haveContactModelDefined", &CS::haveContactModelDefined,
"haveContactModelDefined check that all the contact patch have a contact_model defined")
.def("haveZMPtrajectories", &CS::haveZMPtrajectories,
"check that all the contact phases have a ZMP trajectory.")
.def("getAllEffectorsInContact", &getAllEffectorsInContactAsList,
......@@ -260,7 +262,6 @@ struct ContactSequencePythonVisitor : public bp::def_visitor<ContactSequencePyth
static bp::list getAllEffectorsInContactAsList(CS& self) {
return toPythonList<std::string>(self.getAllEffectorsInContact());
}
};
} // namespace python
} // namespace multicontact_api
......
......@@ -13,20 +13,18 @@ namespace bp = boost::python;
template <typename Derived>
struct cs_pickle_suite : bp::pickle_suite {
static bp::object getstate (const Derived& cs) {
static bp::object getstate(const Derived& cs) {
std::ostringstream os;
boost::archive::text_oarchive oa(os);
oa << cs;
return bp::str(os.str());
}
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);
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;
}
};
......
// 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 @@
#define __multicontact_api_scenario_contact_patch_hpp__
#include "multicontact-api/scenario/fwd.hpp"
#include "multicontact-api/scenario/contact-model.hpp"
#include <pinocchio/spatial/se3.hpp>
#include "multicontact-api/serialization/archive.hpp"
#include "multicontact-api/serialization/spatial.hpp"
......@@ -15,29 +16,31 @@ struct ContactPatchTpl : public serialization::Serializable<ContactPatchTpl<_Sca
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
typedef _Scalar Scalar;
typedef ContactModelTpl<_Scalar> ContactModel;
typedef pinocchio::SE3Tpl<Scalar, 0> SE3;
/// \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.
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
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
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; }
SE3& placement() { return m_placement; }
const Scalar& friction() const { return m_mu; }
Scalar& friction() { return m_mu; }
const Scalar& friction() const { return m_contact_model.m_mu; }
Scalar& friction() { return m_contact_model.m_mu; }
template <typename S2>
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>
......@@ -46,7 +49,7 @@ struct ContactPatchTpl : public serialization::Serializable<ContactPatchTpl<_Sca
}
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>
......@@ -55,11 +58,12 @@ struct ContactPatchTpl : public serialization::Serializable<ContactPatchTpl<_Sca
return os;
}
/// \brief Contact model of this contact
ContactModel m_contact_model;
protected:
/// \brief Placement of the contact patch
SE3 m_placement;
/// \brief friction coefficient for this contact
Scalar m_mu;
private:
// Serialization of the class
......@@ -68,13 +72,13 @@ struct ContactPatchTpl : public serialization::Serializable<ContactPatchTpl<_Sca
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);
ar& boost::serialization::make_nvp("contact_model", m_contact_model);
}
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);
ar >> boost::serialization::make_nvp("contact_model", m_contact_model);
}
BOOST_SERIALIZATION_SPLIT_MEMBER() // why is it required ? using only serialize() lead to compilation error,
......
......@@ -818,6 +818,24 @@ struct ContactSequenceTpl : public serialization::Serializable<ContactSequenceTp
return true;
}
/**
* @brief haveContactModelDefined check that all the contact patch have a contact_model defined
* @return
*/
bool haveContactModelDefined() const {
size_t i = 0;
for (const ContactPhase& phase : m_contact_phases) {
for (const std::string& eeName : phase.effectorsInContact()) {
if (phase.contactPatches().at(eeName).m_contact_model.m_contact_type == ContactType::CONTACT_UNDEFINED) {
std::cout << "ContactModel not defined for phase " << i << " and effector " << eeName << std::endl;
return false;
}
}
++i;
}
return true;
}
/**
* @brief haveZMPtrajectories check that all the contact phases have a zmp trajectory
* @return
......
......@@ -26,9 +26,10 @@ struct ContactSequenceTpl;
typedef ContactSequenceTpl<ContactPhase> ContactSequence;
template <typename Scalar>
struct ContactModelPlanarTpl;
typedef ContactModelPlanarTpl<double> ContactModelPlanar;
struct ContactModelTpl;
typedef ContactModelTpl<double> ContactModel;
enum ContactType { CONTACT_UNDEFINED, CONTACT_PLANAR, CONTACT_POINT };
enum ConicType { CONIC_SOWC, CONIC_DOUBLE_DESCRIPTION, CONIC_UNDEFINED };
} // namespace scenario
......
......@@ -143,14 +143,14 @@ BOOST_AUTO_TEST_CASE(step_in_place_quasistatic_WB) {
BOOST_AUTO_TEST_CASE(walk_20cm) {
ContactSequence cs;
cs.loadFromBinary(path + "walk_20cm.cs");
BOOST_CHECK_EQUAL(cs.size(), 15);
BOOST_CHECK_EQUAL(cs.size(), 23);
BOOST_CHECK(cs.haveConsistentContacts());
}
BOOST_AUTO_TEST_CASE(walk_20cm_COM) {
ContactSequence cs;
cs.loadFromBinary(path + "walk_20cm_COM.cs");
BOOST_CHECK_EQUAL(cs.size(), 15);
BOOST_CHECK_EQUAL(cs.size(), 23);
BOOST_CHECK(cs.haveConsistentContacts());
BOOST_CHECK(cs.haveTimings());
BOOST_CHECK(cs.haveCentroidalValues());
......@@ -160,18 +160,18 @@ BOOST_AUTO_TEST_CASE(walk_20cm_COM) {
BOOST_AUTO_TEST_CASE(walk_20cm_REF) {
ContactSequence cs;
cs.loadFromBinary(path + "walk_20cm_REF.cs");
BOOST_CHECK_EQUAL(cs.size(), 15);
BOOST_CHECK_EQUAL(cs.size(), 23);
BOOST_CHECK(cs.haveConsistentContacts());
BOOST_CHECK(cs.haveTimings());
BOOST_CHECK(cs.haveCentroidalValues());
BOOST_CHECK(cs.haveCentroidalTrajectories());
BOOST_CHECK(cs.haveEffectorsTrajectories(1e-2));
BOOST_CHECK(cs.haveEffectorsTrajectories());
}