Commit a887ef40 authored by Guilhem Saurel's avatar Guilhem Saurel
Browse files

Merge branch 'v2' into 'master'

V2

See merge request loco-3d/multicontact-api!11
parents be639a5c cb7fa82e
Pipeline #8374 passed with stage
in 43 minutes and 35 seconds
......@@ -51,8 +51,8 @@ struct SOCPythonVisitor : public boost::python::def_visitor<SOCPythonVisitor<SOC
.def(SerializableVisitor<SOC>());
// Expose related matrix types
eigenpy::enableEigenPySpecific<MatrixD, MatrixD>();
eigenpy::enableEigenPySpecific<VectorD, VectorD>();
ENABLE_SPECIFIC_MATRIX_TYPE(MatrixD);
ENABLE_SPECIFIC_MATRIX_TYPE(VectorD);
}
protected:
......
......@@ -19,36 +19,21 @@ template <typename ContactPatch>
struct ContactPatchPythonVisitor : public boost::python::def_visitor<ContactPatchPythonVisitor<ContactPatch> > {
typedef typename ContactPatch::Scalar Scalar;
typedef typename ContactPatch::SE3 SE3;
typedef typename ContactPatch::LinearWrenchCone LWC;
typedef typename ContactPatch::ContactModel ContactModel;
template <class PyClass>
void visit(PyClass& cl) const {
cl.def(bp::init<SE3>(bp::arg("placement"), "Init with a given placement."))
.def(bp::init<ContactPatch>(bp::args("other"), "Copy contructor."))
cl.def(bp::init<>(bp::arg(""), "Default constructor."))
.def(bp::init<SE3>(bp::arg("placement"), "Init with a given placement."))
.def(bp::init<SE3, Scalar>(bp::args("placement", "friction"),
"Init with a given placement and friction coefficient."))
.def(bp::init<ContactPatch>(bp::arg("other"), "Copy contructor."))
.add_property("placement",
bp::make_function((SE3 & (ContactPatch::*)(void)) & ContactPatch::placement,
bp::return_internal_reference<>()),
&setPlacement, "Placement of the patch.")
.add_property("oMcp",
bp::make_function((SE3 & (ContactPatch::*)(void)) & ContactPatch::placement,
bp::return_internal_reference<>()),
&setPlacement, "Placement of the contact model with respect to the world.")
.add_property("active", bp::make_function((bool (ContactPatch::*)(void) const) & ContactPatch::active),
&setActive, "Active property.")
.add_property(
"lwc",
bp::make_function((LWC & (ContactPatch::*)(void)) & ContactPatch::lwc, bp::return_internal_reference<>()),
&setLwc, "Linear Wrench Cone associated to the patch.")
.add_property("contactModel",
bp::make_function((ContactModel & (ContactPatch::*)(void)) & ContactPatch::contactModel,
bp::return_internal_reference<>()),
&setContactModel, "Contact Model associated to the patch.")
.add_property("contactModelPlacement",
bp::make_function((SE3 & (ContactPatch::*)(void)) & ContactPatch::contactModelPlacement,
bp::return_internal_reference<>()),
&setContactModelPlacement, "Placement of the patch.")
// getter require to use "make_function" to pass the return_internal_reference policy (return ref
// to custom object)
bp::make_function(&getPlacement, bp::return_internal_reference<>()), &setPlacement,
"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(bp::self == bp::self)
.def(bp::self != bp::self)
.def("copy", &copy, "Returns a copy of *this.");
......@@ -63,15 +48,11 @@ struct ContactPatchPythonVisitor : public boost::python::def_visitor<ContactPatc
}
protected:
// define setter and getter
static SE3& getPlacement(ContactPatch& self) { return self.placement(); }
static void setPlacement(ContactPatch& self, const SE3& placement) { self.placement() = placement; }
static void setContactModelPlacement(ContactPatch& self, const SE3& placement) {
self.contactModelPlacement() = placement;
}
static void setActive(ContactPatch& self, const bool value) { self.active() = value; }
static void setContactModel(ContactPatch& self, const ContactModel& contactModel) {
self.contactModel() = contactModel;
}
static void setLwc(ContactPatch& self, const LWC& lwc) { self.lwc() = lwc; }
static Scalar getFriction(ContactPatch& self) { return self.friction(); }
static void setFriction(ContactPatch& self, const Scalar& friction) { self.friction() = friction; }
static ContactPatch copy(const ContactPatch& self) { return ContactPatch(self); }
};
} // namespace python
......
// Copyright (c) 2015-2018, CNRS
// Authors: Justin Carpentier <jcarpent@laas.fr>
#ifndef __multicontact_api_python_scenario_contact_phase_humanoid_hpp__
#define __multicontact_api_python_scenario_contact_phase_humanoid_hpp__
#include <eigenpy/eigenpy.hpp>
#include "multicontact-api/scenario/contact-phase-humanoid.hpp"
#include "multicontact-api/scenario/contact-phase.hpp"
#include "multicontact-api/bindings/python/container/visitor.hpp"
#include "multicontact-api/bindings/python/container/array.hpp"
#include <pinocchio/bindings/python/utils/std-aligned-vector.hpp>
namespace multicontact_api {
namespace python {
namespace bp = boost::python;
template <typename ContactPhase>
struct ContactPhaseHumanoidPythonVisitor
: public boost::python::def_visitor<ContactPhaseHumanoidPythonVisitor<ContactPhase> > {
typedef typename ContactPhase::Scalar Scalar;
typedef typename ContactPhase::Base ContactPhase4;
typedef typename ContactPhase::ContactPatch ContactPatch;
typedef typename ContactPhase::StateVector StateVector;
typedef typename ContactPhase::ControlVector ControlVector;
typedef typename ContactPhase::ConfigurationVector ConfigurationVector;
typedef typename ContactPhase::VectorStateVector VectorStateVector;
typedef typename ContactPhase::VectorControlVector VectorControlVector;
typedef typename ContactPhase::VectorConfigurationVector VectorConfigurationVector;
typedef typename ContactPhase::VectorForce VectorForce;
typedef typename ContactPhase::Force Force;
template <class PyClass>
void visit(PyClass& cl) const {
cl.def(bp::init<>(bp::arg(""), "Default constructor."))
.def(bp::init<ContactPhase>(bp::args("other"), "Copy contructor."))
.add_property("RF_patch", bp::make_function(&getRFpatch, bp::return_internal_reference<>()),
bp::make_function(&setRFpatch))
.add_property("LF_patch", bp::make_function(&getLFpatch, bp::return_internal_reference<>()),
bp::make_function(&setLFpatch))
.add_property("RH_patch", bp::make_function(&getRHpatch, bp::return_internal_reference<>()),
bp::make_function(&setRHpatch))
.add_property("LH_patch", bp::make_function(&getLHpatch, bp::return_internal_reference<>()),
bp::make_function(&setLHpatch))
.add_property("init_state", &getInitState, &setInitState, "Initial state of the phase.")
.add_property("final_state", &getFinalState, &setFinalState, "Final state of the phase.")
.add_property("state_trajectory",
bp::make_getter(&ContactPhase::m_state_trajectory, bp::return_internal_reference<>()))
.add_property("dot_state_trajectory",
bp::make_getter(&ContactPhase::m_dot_state_trajectory, bp::return_internal_reference<>()))
.add_property("control_trajectory",
bp::make_getter(&ContactPhase::m_control_trajectory, bp::return_internal_reference<>()))
.add_property("time_trajectory",
bp::make_getter(&ContactPhase::m_time_trajectory, bp::return_internal_reference<>()))
.add_property("objective_trajectory",
bp::make_getter(&ContactPhase::m_objective_trajectory, bp::return_internal_reference<>()))
.def_readwrite("reference_configurations", &ContactPhase::m_reference_configurations)
.def_readwrite("raw_control_trajectory", &ContactPhase::m_raw_control_trajectory)
.def_readwrite("angular_momentum_ref", &ContactPhase::m_angular_momentum_ref)
.def_readwrite("com_ref", &ContactPhase::m_com_ref)
.def_readwrite("vcom_ref", &ContactPhase::m_vcom_ref)
.def_readwrite("forces_ref", &ContactPhase::m_forces_ref)
.add_property("contact_forces_trajectories", bp::make_array(&ContactPhase::m_contact_forces_trajectories))
.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 Phase for a Humanoid robot definied by its 4 end effectors.";
bp::class_<ContactPhase, bp::bases<ContactPhase4> >(class_name.c_str(), doc.c_str(), bp::no_init)
.def(ContactPhaseHumanoidPythonVisitor<ContactPhase>());
// Expose related quantities
eigenpy::enableEigenPySpecific<StateVector, StateVector>();
eigenpy::enableEigenPySpecific<ConfigurationVector, ConfigurationVector>();
VectorPythonVisitor<VectorStateVector, true>::expose("VectorStateVector");
VectorPythonVisitor<VectorControlVector, true>::expose("VectorControlVector");
VectorPythonVisitor<VectorConfigurationVector, true>::expose("VectorConfigurationVector");
VectorPythonVisitor<VectorForce, true>::expose("VectorForce");
}
protected:
static ContactPatch& getRFpatch(ContactPhase& self) { return self.RF_patch; }
static void setRFpatch(ContactPhase& self, const ContactPatch& contact_patch) { self.RF_patch = contact_patch; }
static ContactPatch& getLFpatch(ContactPhase& self) { return self.LF_patch; }
static void setLFpatch(ContactPhase& self, const ContactPatch& contact_patch) { self.LF_patch = contact_patch; }
static ContactPatch& getRHpatch(ContactPhase& self) { return self.RH_patch; }
static void setRHpatch(ContactPhase& self, const ContactPatch& contact_patch) { self.RH_patch = contact_patch; }
static ContactPatch& getLHpatch(ContactPhase& self) { return self.LH_patch; }
static void setLHpatch(ContactPhase& self, const ContactPatch& contact_patch) { self.LH_patch = contact_patch; }
static StateVector getInitState(const ContactPhase& self) { return self.m_init_state; }
static void setInitState(ContactPhase& self, const StateVector& init_state) { self.m_init_state = init_state; }
static StateVector getFinalState(const ContactPhase& self) { return self.m_final_state; }
static void setFinalState(ContactPhase& self, const StateVector& final_state) { self.m_final_state = final_state; }
static ContactPhase copy(const ContactPhase& self) { return ContactPhase(self); }
};
} // namespace python
} // namespace multicontact_api
#endif // ifndef __multicontact_api_python_scenario_contact_phase_humanoid_hpp__
......@@ -10,7 +10,6 @@
#include "multicontact-api/scenario/contact-sequence.hpp"
#include "multicontact-api/bindings/python/serialization/archive.hpp"
#include "multicontact-api/bindings/python/container/visitor.hpp"
namespace multicontact_api {
namespace python {
......@@ -18,25 +17,223 @@ namespace python {
namespace bp = boost::python;
template <typename CS>
struct ContactSequencePythonVisitor : public boost::python::def_visitor<ContactSequencePythonVisitor<CS> > {
struct ContactSequencePythonVisitor : public bp::def_visitor<ContactSequencePythonVisitor<CS> > {
typedef typename CS::ContactPhaseVector ContactPhaseVector;
typedef typename CS::MSIntervalDataVector MSIntervalDataVector;
typedef typename CS::MSIntervalData MSIntervalData;
BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(cs_breakContact_overloads, CS::breakContact, 1, 2)
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)
template <class PyClass>
void visit(PyClass& cl) const {
cl.def(bp::init<size_t>(bp::arg("size"), "Default constructor from a given size."))
.def(bp::init<>(bp::arg(""), "Default constructor."))
.def(bp::init<CS>(bp::args("other"), "Copy contructor."))
.add_property("contact_phases", bp::make_getter(&CS::m_contact_phases, bp::return_internal_reference<>()))
.def("size", &CS::size, "Return the size of the contact sequence.")
.def("resize", &CS::resize, bp::arg("size"), "Resize the vector of ContactPhases.")
.def("append", &CS::append, bp::arg("ContactPhase"),
"Add the given ContactPhase at the end of the sequence. \n"
"Return the new id of this ContactPhase inside the sequence.")
.add_property("contactPhases", bp::make_getter(&CS::m_contact_phases, bp::return_internal_reference<>()),
"Vector of Contact Phases contained in the sequence")
.def("breakContact", &CS::breakContact,
cs_breakContact_overloads(
(bp::arg("eeName"), bp::arg("phaseDuration") = -1),
" Add a new contactPhase at the end of the current ContactSequence,"
"The new ContactPhase have the same ContactPatchs as the last phase of the sequence,"
"with the exeption of the given contact removed."
"It copy all the 'final' values of the last phase as 'initial' values of the new phase."
"It also set the duration of the previous last phase. \n"
"phaseDuration : if provided, the duration of the previous last phase of the sequence is set to this "
"value"
"(it is thus the duration BEFORE breaking the contact) \n"
"Raise value_error if the phaseDuration is provided but the last phase do not have a time-range "
"defined.\n"
"Raise value_error if eeName is not in contact in the last phase of the sequence.\n"))
.def("createContact", &CS::createContact,
cs_createContact_overloads(
(bp::arg("eeName"), bp::arg("contactPatch"), bp::arg("phaseDuration") = -1),
"Add a new contactPhase at the end of the current ContactSequence,"
"The new ContactPhase have the same ContactPatchs as the last phase of the sequence,"
"with the exeption of the given contact added.\n"
"phaseDuration: if provided, the duration of the previous last phase of the sequence is set to this "
"value"
"(it is thus the duration BEFORE creating the contact)\n"
"Raise value_error if the phaseDuration is provided but the last phase do not have a time-range "
"defined\n"
"Raise value_error if eeName is already in contact in the last phase of the sequence"))
.def("moveEffectorToPlacement", &CS::moveEffectorToPlacement,
cs_moveEffectorToPlacement_overloads(
(bp::arg("eeName"), bp::arg("placement"), bp::arg("durationBreak") = -1,
bp::arg("durationCreate") = -1),
"Add two new phases at the end of the current ContactSequence:\n"
"- it break the contact with eeName\n"
"- it create the contact with eeName at the given placement.\n"
"It copy all the 'final' values of the last phase as 'initial' values of the new phase."
"It also set the duration of the previous last phase.\n"
"placement: the new placement for the contact of eeName, defined as a pinocchio::SE3\n"
"durationBreak: the duration of the previous last phase of the sequence"
"(it is thus the duration BEFORE breaking the contact)\n"
"durationCreate: the duration of the first new ContactPhase"
"(it is thus the duration BEFORE creating the contact)\n"
"Raise value_error if the phaseDuration is provided but the last phase do not have a time-range "
"defined\n"
"Raise value_error if eeName is not in contact in the last phase of the sequence\n"))
.def(
"moveEffectorOf", &CS::moveEffectorOf,
cs_moveEffectorOf_overloads(
(bp::arg("eeName"), bp::arg("transform"), bp::arg("durationBreak") = -1,
bp::arg("durationCreate") = -1),
"Similar to moveEffectorToPlacement"
"exept that the new placement is defined from the previous placement and a given transform applied.\n"
"transform: the transform applied to the placement of the contact in the last phase of the sequence.\n"
"durationBreak: the duration of the previous last phase of the sequence"
"(it is thus the duration BEFORE breaking the contact)\n"
"durationCreate: the duration of the first new ContactPhase"
"(it is thus the duration BEFORE creating the contact)\n"
"Raise value_error if the phaseDuration is provided but the last phase do not have a time-range "
"defined\n"
"Raise value_error if eeName is not in contact in the last phase of the sequence\n"))
.def("haveTimings", &CS::haveTimings,
"Check if all the time intervals are defined and consistent"
"(ie. the time always increase and the final time of one phase is equal to the initial one of the newt "
"phase) \n"
"Return true if the sequence is consistent, false otherwise")
.def("haveConsistentContacts", &CS::haveConsistentContacts,
"check that there is always one contact change between adjacent phases in the sequence.\n"
"and that there isn't any phase without any contact.")
.def("haveCOMvalues", &CS::haveCOMvalues,
"Check that the initial and final CoM position values are defined for all phases.\n"
"Also check that the initial values of one phase correspond to the final values of the previous ones.")
.def("haveAMvalues", &CS::haveAMvalues,
"Check that the initial and final AM values are defined for all phases.\n"
"Also check that the initial values of one phase correspond to the final values of the previous ones.")
.def("haveCentroidalValues", &CS::haveCentroidalValues,
"Check that the initial and final CoM position and AM values are defined for all phases.\n"
"Also check that the initial values of one phase correspond to the final values of the previous ones.")
.def("haveConfigurationsValues", &CS::haveConfigurationsValues,
"Check that the initial and final configuration are defined for all phases.\n"
"Also check that the initial values of one phase correspond to the final values of the previous ones.")
.def("haveCOMtrajectories", &CS::haveCOMtrajectories,
"check that a c, dc and ddc trajectories are defined for each phases.\n"
"Also check that the time interval of this trajectories matches the one of the phase.\n"
"and that the trajectories start and end and the correct values defined in each phase.")
.def("haveAMtrajectories", &CS::haveAMtrajectories,
"check that a L and dL trajectories are defined for each phases.\n"
"Also check that the time interval of this trajectories matches the one of the phase.\n"
"and that the trajectories start and end and the correct values defined in each phase.")
.def("haveCentroidalTrajectories", &CS::haveCentroidalTrajectories,
"check that all centroidal trajectories are defined for each phases.\n"
"Also check that the time interval of this trajectories matches the one of the phase.\n"
"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()),
"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."))
.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"
"and that the trajectories start and end and the correct values defined in each phase.")
.def("haveJointsDerivativesTrajectories", &CS::haveJointsDerivativesTrajectories,
"Check that a dq and ddq trajectories are defined for each phases.\n"
"Also check that the time interval of this trajectories matches the one of the phase.\n"
"and that the trajectories start and end and the correct values defined in each phase.")
.def("haveTorquesTrajectories", &CS::haveTorquesTrajectories,
"Check that a joint torque trajectories are defined for each phases.\n"
"Also check that the time interval of this trajectories matches the one of the phase.\n"
"and that the trajectories start and end and the correct values defined in each phase")
.def("haveContactForcesTrajectories", &CS::haveContactForcesTrajectories,
"Check that a contact force trajectory exist for each active contact.\n"
"Also check that the time interval of this trajectories matches the one of the phase.\n"
"and that the trajectories start and end and the correct values defined in each phase.")
.def("haveRootTrajectories", &CS::haveRootTrajectories,
"check that a root trajectory exist for each contact phases.\n"
"Also check that it start and end at the correct time interval.")
.def("haveFriction", &CS::haveFriction,
"check that all the contact patch used in the sequence have"
"a friction coefficient initialized.")
.def("haveZMPtrajectories", &CS::haveZMPtrajectories,
"check that all the contact phases have a ZMP trajectory.")
.def("getAllEffectorsInContact",&getAllEffectorsInContactAsList,
"return a list of names of all the effectors used to create contacts during the sequence")
.def("concatenateCtrajectories", &CS::concatenateCtrajectories,
"Return a piecewise curve wchich is the concatenation of the m_c curves"
" for each contact phases in the sequence.")
.def("concatenateDCtrajectories", &CS::concatenateDCtrajectories,
"Return a piecewise curve wchich is the concatenation of the m_dc curves"
" for each contact phases in the sequence.")
.def("concatenateDDCtrajectories", &CS::concatenateDDCtrajectories,
"Return a piecewise curve wchich is the concatenation of the m_ddc curves"
" for each contact phases in the sequence.")
.def("concatenateLtrajectories", &CS::concatenateLtrajectories,
"Return a piecewise curve wchich is the concatenation of the m_L curves"
" for each contact phases in the sequence.")
.def("concatenateDLtrajectories", &CS::concatenateDLtrajectories,
"Return a piecewise curve wchich is the concatenation of the m_dL curves"
" for each contact phases in the sequence.")
.def("concatenateZMPtrajectories", &CS::concatenateZMPtrajectories,
"Return a piecewise curve wchich is the concatenation of the m_zmp curves"
" for each contact phases in the sequence.")
.def("concatenateWrenchTrajectories", &CS::concatenateWrenchTrajectories,
"Return a piecewise curve wchich is the concatenation of the m_wrench curves"
" for each contact phases in the sequence.")
.def("concatenateQtrajectories", &CS::concatenateQtrajectories,
"Return a piecewise curve wchich is the concatenation of the m_q curves"
" for each contact phases in the sequence.")
.def("concatenateDQtrajectories", &CS::concatenateDQtrajectories,
"Return a piecewise curve wchich is the concatenation of the m_dq curves"
" for each contact phases in the sequence.")
.def("concatenateDDQtrajectories", &CS::concatenateDDQtrajectories,
"Return a piecewise curve wchich is the concatenation of the m_ddq curves"
" for each contact phases in the sequence.")
.def("concatenateTauTrajectories", &CS::concatenateTauTrajectories,
"Return a piecewise curve wchich is the concatenation of the m_tau curves"
" for each contact phases in the sequence.")
.def("concatenateRootTrajectories", &CS::concatenateRootTrajectories,
"Return a piecewise curve wchich is the concatenation of the m_root curves"
" for each contact phases in the sequence.")
.def("concatenateEffectorTrajectories", &CS::concatenateEffectorTrajectories,
bp::arg("eeName"),
"Return a piecewise curve which is the concatenation"
"of the effectors trajectories curves for the given effector"
"for each contact phases in the sequence.\n"
"During the phases where no effector trajectories are defined,"
"the trajectory is constant with the value of"
"the last phase where it was defined.")
.def("concatenateContactForceTrajectories", &CS::concatenateContactForceTrajectories,
bp::arg("eeName"),
"Return a piecewise curve which"
"is the concatenation of the contact forces for the given effector"
"for each contact phases in the sequence.\n"
"During the phases where no contact forces are defined,"
"the trajectory is constant with the value of 0.")
.def("concatenateNormalForceTrajectories", &CS::concatenateNormalForceTrajectories,
bp::arg("eeName"),
"Return a piecewise curve which"
"is the concatenation of the contact normal forces for the given effector"
"for each contact phases in the sequence.\n"
"During the phases where no contact normal forces are defined,"
"the trajectory is constant with the value of 0.")
.def("phaseIdAtTime", &CS::phaseIdAtTime,
bp::arg("time"),
"return the index of a phase in the sequence such that "
"phase.timeInitial <= t < phase.timeFinal \n"
"if t equal to the last phase timeFinal, this index is returned.")
.def("phaseAtTime", &CS::phaseAtTime,
bp::arg("time"), bp::return_internal_reference<>(),
"return a phase of the sequence such that "
"phase.timeInitial <= t < phase.timeFinal \n"
"if t equal to the last phase timeFinal, this index is returned.")
.def(bp::self == bp::self)
.def(bp::self != bp::self)
.def("size", &CS::size, "Size of the contact sequence.")
.def_readwrite("ms_interval_data", &CS::m_ms_interval_data)
.def_readwrite("conic_type", &CS::m_conic_type);
.def("copy", &copy, "Returns a copy of *this.");
}
static void expose(const std::string& class_name) {
......@@ -45,9 +242,27 @@ struct ContactSequencePythonVisitor : public boost::python::def_visitor<ContactS
.def(ContactSequencePythonVisitor<CS>())
.def(SerializableVisitor<CS>());
// Expose related vector
VectorPythonVisitor<ContactPhaseVector>::expose("ContactPhaseVector");
pinocchio::python::StdAlignedVectorPythonVisitor<MSIntervalData, true>::expose("MSIntervalDataVector");
bp::class_<ContactPhaseVector>("ContactPhaseVector").def(bp::vector_indexing_suite<ContactPhaseVector>());
}
protected:
static CS copy(const CS& self) { return CS(self); }
// Converts a C++ vector to a python list
// Note : lot of overhead, should not be used for large vector and/or operations called frequently.
// prefer the direct bindings with std_vector_strings for this cases.
template <class T>
static bp::list toPythonList(std::vector<T> vector) {
typename std::vector<T>::const_iterator iter;
boost::python::list list;
for (iter = vector.begin(); iter != vector.end(); ++iter) {
list.append(*iter);
}
return list;
}
static bp::list getAllEffectorsInContactAsList(CS& self) {
return toPythonList<std::string>(self.getAllEffectorsInContact());
}
};
} // namespace python
......
// Copyright (c) 2015-2018, CNRS
// Authors: Justin Carpentier <jcarpent@laas.fr>
#ifndef __multicontact_api_python_scenerario_enum_hpp__
#define __multicontact_api_python_scenerario_enum_hpp__
#include <boost/python.hpp>
#include <string>
namespace multicontact_api {
namespace python {
void exposeEnumHumanoidPhaseType(const std::string& enum_name);
}
} // namespace multicontact_api
#endif // ifndef __multicontact_api_python_scenerario_enum_hpp__
......@@ -14,11 +14,11 @@ void exposeScenarioEnums();
void exposeContactModels();
inline void exposeScenario() {
exposeScenarioEnums();
exposeContactModels();
exposeContactPatch();
exposeContactPhase();
exposeContactSequence();
exposeScenarioEnums();
exposeContactModels();
}
} // namespace python
......
// Copyright (c) 2015-2018, CNRS
// Authors: Justin Carpentier <jcarpent@laas.fr>
#ifndef __multicontact_api_python_scenario_ms_interval_hpp__
#define __multicontact_api_python_scenario_ms_interval_hpp__
#include <eigenpy/eigenpy.hpp>
#include "multicontact-api/scenario/contact-phase-humanoid.hpp"
#include "multicontact-api/scenario/contact-phase.hpp"
#include "multicontact-api/bindings/python/container/visitor.hpp"
#include "multicontact-api/bindings/python/container/array.hpp"
#include <pinocchio/bindings/python/utils/std-aligned-vector.hpp>
namespace multicontact_api {
namespace python {
namespace bp = boost::python;
template <typename MSInterval>
struct MSIntervalPythonVisitor : public boost::python::def_visitor<MSIntervalPythonVisitor<MSInterval> > {
typedef typename MSInterval::TimeVector TimeVector;
typedef typename MSInterval::StateVectorTrajectory StateVectorTrajectory;
typedef typename MSInterval::ControlVectorTrajectory ControlVectorTrajectory;
template <class PyClass>
void visit(PyClass& cl) const {
cl.def(bp::init<>(bp::arg(""), "Default constructor."))
.def(bp::init<MSInterval>(bp::args("other"), "Copy contructor."))
.add_property("time_trajectory", bp::make_function(&time_trajectory, bp::return_internal_reference<>()))
.add_property("state_trajectory", bp::make_function(&state_trajectory, bp::return_internal_reference<>()))
.add_property("dot_state_trajectory",
bp::make_function(&dot_state_trajectory, bp::return_internal_reference<>()))
.add_property("control_trajectory", bp::make_function(&control_trajectory, bp::return_internal_reference<>()))
.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 =
"Multiple Shootin inveral data: state and dot state trajectories, time trajectory and control trajectory.";
bp::class_<MSInterval>(class_name.c_str(), doc.c_str(), bp::no_init).def(MSIntervalPythonVisitor<MSInterval>());
}
protected: