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

Merge pull request #12 from loco-3d/devel

Devel
parents d217188f 8c81b3e9
Pipeline #9968 passed with stage
in 11 minutes and 22 seconds
No preview for this file type
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.");
......
......@@ -24,7 +24,7 @@ struct ContactSequencePythonVisitor : public bp::def_visitor<ContactSequencePyth
BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(cs_createContact_overloads, CS::createContact, 2, 3)
BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(cs_moveEffectorToPlacement_overloads, CS::moveEffectorToPlacement, 2, 4)
BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(cs_moveEffectorOf_overloads, CS::moveEffectorOf, 2, 4)
BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(cs_haveEffectorTrajectories_overloads, CS::haveEffectorsTrajectories, 0, 1)
BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(cs_haveEffectorTrajectories_overloads, CS::haveEffectorsTrajectories, 0, 2)
template <class PyClass>
void visit(PyClass& cl) const {
......@@ -130,14 +130,16 @@ struct ContactSequencePythonVisitor : public bp::def_visitor<ContactSequencePyth
"and that the trajectories start and end and the correct values defined in each phase.")
.def("haveEffectorsTrajectories", &CS::haveEffectorsTrajectories,
cs_haveEffectorTrajectories_overloads(
(bp::args("precision_treshold") = Eigen::NumTraits<typename CS::Scalar>::dummy_precision()),
(bp::args("precision_treshold") = Eigen::NumTraits<typename CS::Scalar>::dummy_precision(),
bp::args("use_rotation") = true),
"check that for each phase preceeding a contact creation,"
"an SE3 trajectory is defined for the effector that will be in contact.\n"
"Also check that this trajectory is defined on the time-interval of the phase.\n"
"Also check that the trajectory correctly end at the placement defined for the contact in the next "
"phase.\n"
"If this effector was in contact in the previous phase,"
"it check that the trajectory start at the previous contact placement."))
"it check that the trajectory start at the previous contact placement.\n"
"If use_rotation == false, only the translation part of the transforms are compared. "))
.def("haveJointsTrajectories", &CS::haveJointsTrajectories,
"Check that a q trajectory is defined for each phases.\n"
"Also check that the time interval of this trajectories matches the one of the phase.\n"
......@@ -160,6 +162,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,
......
......@@ -11,6 +11,24 @@ namespace python {
namespace bp = boost::python;
template <typename Derived>
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 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>
struct SerializableVisitor : public boost::python::def_visitor<SerializableVisitor<Derived> > {
template <class PyClass>
......@@ -20,7 +38,8 @@ struct SerializableVisitor : public boost::python::def_visitor<SerializableVisit
.def("saveAsXML", &Derived::saveAsXML, bp::args("filename", "tag_name"), "Saves *this inside a XML file.")
.def("loadFromXML", &Derived::loadFromXML, bp::args("filename", "tag_name"), "Loads *this from a XML file.")
.def("saveAsBinary", &Derived::saveAsBinary, bp::args("filename"), "Saves *this inside a binary file.")
.def("loadFromBinary", &Derived::loadFromBinary, bp::args("filename"), "Loads *this from a binary file.");
.def("loadFromBinary", &Derived::loadFromBinary, bp::args("filename"), "Loads *this from a binary file.")
.def_pickle(cs_pickle_suite<Derived>());
}
};
} // namespace python
......
// 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,
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment