contact-sequence.hpp
Go to the documentation of this file.
1 // Copyright (c) 2015-2018, CNRS
2 // Authors: Justin Carpentier <jcarpent@laas.fr>
3 
4 #ifndef __multicontact_api_python_scenario_contact_sequence_hpp__
5 #define __multicontact_api_python_scenario_contact_sequence_hpp__
6 
7 #include <eigenpy/eigenpy.hpp>
8 #include <pinocchio/bindings/python/utils/std-aligned-vector.hpp>
9 #include <pinocchio/fwd.hpp>
10 
13 
14 namespace multicontact_api {
15 namespace python {
16 
17 namespace bp = boost::python;
18 
19 template <typename CS>
21  : public bp::def_visitor<ContactSequencePythonVisitor<CS> > {
22  typedef typename CS::ContactPhaseVector ContactPhaseVector;
23 
24  BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(cs_breakContact_overloads,
25  CS::breakContact, 1, 2)
26  BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(cs_createContact_overloads,
27  CS::createContact, 2, 3)
28  BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(cs_moveEffectorToPlacement_overloads,
29  CS::moveEffectorToPlacement, 2, 4)
30  BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(cs_moveEffectorOf_overloads,
31  CS::moveEffectorOf, 2, 4)
32  BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(cs_haveEffectorTrajectories_overloads,
33  CS::haveEffectorsTrajectories, 0, 2)
34 
35  template <class PyClass>
36  void visit(PyClass& cl) const {
37  cl.def(bp::init<size_t>(bp::arg("size"),
38  "Default constructor from a given size."))
39  .def(bp::init<>(bp::arg(""), "Default constructor."))
40  .def(bp::init<CS>(bp::args("other"), "Copy contructor."))
41  .def("size", &CS::size, "Return the size of the contact sequence.")
42  .def("resize", &CS::resize, bp::arg("size"),
43  "Resize the vector of ContactPhases.")
44  .def("append", &CS::append, bp::arg("ContactPhase"),
45  "Add the given ContactPhase at the end of the sequence. \n"
46  "Return the new id of this ContactPhase inside the sequence.")
47  .add_property("contactPhases",
48  bp::make_getter(&CS::m_contact_phases,
49  bp::return_internal_reference<>()),
50  "Vector of Contact Phases contained in the sequence")
51  .def("breakContact", &CS::breakContact,
52  cs_breakContact_overloads(
53  (bp::arg("eeName"), bp::arg("phaseDuration") = -1),
54  " Add a new contactPhase at the end of the current "
55  "ContactSequence,"
56  "The new ContactPhase have the same ContactPatchs as the last "
57  "phase of the sequence,"
58  "with the exeption of the given contact removed."
59  "It copy all the 'final' values of the last phase as "
60  "'initial' values of the new phase."
61  "It also set the duration of the previous last phase. \n"
62  "phaseDuration : if provided, the duration of the previous "
63  "last phase of the sequence is set to this "
64  "value"
65  "(it is thus the duration BEFORE breaking the contact) \n"
66  "Raise value_error if the phaseDuration is provided but the "
67  "last phase do not have a time-range "
68  "defined.\n"
69  "Raise value_error if eeName is not in contact in the last "
70  "phase of the sequence.\n"))
71  .def("createContact", &CS::createContact,
72  cs_createContact_overloads(
73  (bp::arg("eeName"), bp::arg("contactPatch"),
74  bp::arg("phaseDuration") = -1),
75  "Add a new contactPhase at the end of the current "
76  "ContactSequence,"
77  "The new ContactPhase have the same ContactPatchs as the last "
78  "phase of the sequence,"
79  "with the exeption of the given contact added.\n"
80  "phaseDuration: if provided, the duration of the previous "
81  "last phase of the sequence is set to this "
82  "value"
83  "(it is thus the duration BEFORE creating the contact)\n"
84  "Raise value_error if the phaseDuration is provided but the "
85  "last phase do not have a time-range "
86  "defined\n"
87  "Raise value_error if eeName is already in contact in the "
88  "last phase of the sequence"))
89  .def(
90  "moveEffectorToPlacement", &CS::moveEffectorToPlacement,
91  cs_moveEffectorToPlacement_overloads(
92  (bp::arg("eeName"), bp::arg("placement"),
93  bp::arg("durationBreak") = -1, bp::arg("durationCreate") = -1),
94  "Add two new phases at the end of the current "
95  "ContactSequence:\n"
96  "- it break the contact with eeName\n"
97  "- it create the contact with eeName at the given placement.\n"
98  "It copy all the 'final' values of the last phase as 'initial' "
99  "values of the new phase."
100  "It also set the duration of the previous last phase.\n"
101  "placement: the new placement for the contact of eeName, "
102  "defined as a pinocchio::SE3\n"
103  "durationBreak: the duration of the previous last phase of the "
104  "sequence"
105  "(it is thus the duration BEFORE breaking the contact)\n"
106  "durationCreate: the duration of the first new ContactPhase"
107  "(it is thus the duration BEFORE creating the contact)\n"
108  "Raise value_error if the phaseDuration is provided but the "
109  "last phase do not have a time-range "
110  "defined\n"
111  "Raise value_error if eeName is not in contact in the last "
112  "phase of the sequence\n"))
113  .def(
114  "moveEffectorOf", &CS::moveEffectorOf,
115  cs_moveEffectorOf_overloads(
116  (bp::arg("eeName"), bp::arg("transform"),
117  bp::arg("durationBreak") = -1, bp::arg("durationCreate") = -1),
118  "Similar to moveEffectorToPlacement"
119  "exept that the new placement is defined from the previous "
120  "placement and a given transform applied.\n"
121  "transform: the transform applied to the placement of the "
122  "contact in the last phase of the sequence.\n"
123  "durationBreak: the duration of the previous last phase of the "
124  "sequence"
125  "(it is thus the duration BEFORE breaking the contact)\n"
126  "durationCreate: the duration of the first new ContactPhase"
127  "(it is thus the duration BEFORE creating the contact)\n"
128  "Raise value_error if the phaseDuration is provided but the "
129  "last phase do not have a time-range "
130  "defined\n"
131  "Raise value_error if eeName is not in contact in the last "
132  "phase of the sequence\n"))
133  .def("haveTimings", &CS::haveTimings,
134  "Check if all the time intervals are defined and consistent"
135  "(ie. the time always increase and the final time of one phase is "
136  "equal to the initial one of the newt "
137  "phase) \n"
138  "Return true if the sequence is consistent, false otherwise")
139  .def("haveConsistentContacts", &CS::haveConsistentContacts,
140  "check that there is always one contact change between adjacent "
141  "phases in the sequence.\n"
142  "and that there isn't any phase without any contact.")
143  .def("haveCOMvalues", &CS::haveCOMvalues,
144  "Check that the initial and final CoM position values are defined "
145  "for all phases.\n"
146  "Also check that the initial values of one phase correspond to "
147  "the final values of the previous ones.")
148  .def("haveAMvalues", &CS::haveAMvalues,
149  "Check that the initial and final AM values are defined for all "
150  "phases.\n"
151  "Also check that the initial values of one phase correspond to "
152  "the final values of the previous ones.")
153  .def("haveCentroidalValues", &CS::haveCentroidalValues,
154  "Check that the initial and final CoM position and AM values are "
155  "defined for all phases.\n"
156  "Also check that the initial values of one phase correspond to "
157  "the final values of the previous ones.")
158  .def("haveConfigurationsValues", &CS::haveConfigurationsValues,
159  "Check that the initial and final configuration are defined for "
160  "all phases.\n"
161  "Also check that the initial values of one phase correspond to "
162  "the final values of the previous ones.")
163  .def("haveCOMtrajectories", &CS::haveCOMtrajectories,
164  "check that a c, dc and ddc trajectories are defined for each "
165  "phases.\n"
166  "Also check that the time interval of this trajectories matches "
167  "the one of the phase.\n"
168  "and that the trajectories start and end and the correct values "
169  "defined in each phase.")
170  .def("haveAMtrajectories", &CS::haveAMtrajectories,
171  "check that a L and dL trajectories are defined for each phases.\n"
172  "Also check that the time interval of this trajectories matches "
173  "the one of the phase.\n"
174  "and that the trajectories start and end and the correct values "
175  "defined in each phase.")
176  .def("haveCentroidalTrajectories", &CS::haveCentroidalTrajectories,
177  "check that all centroidal trajectories are defined for each "
178  "phases.\n"
179  "Also check that the time interval of this trajectories matches "
180  "the one of the phase.\n"
181  "and that the trajectories start and end and the correct values "
182  "defined in each phase.")
183  .def("haveEffectorsTrajectories", &CS::haveEffectorsTrajectories,
184  cs_haveEffectorTrajectories_overloads(
185  (bp::args("precision_treshold") =
186  Eigen::NumTraits<typename CS::Scalar>::dummy_precision(),
187  bp::args("use_rotation") = true),
188  "check that for each phase preceeding a contact creation,"
189  "an SE3 trajectory is defined for the effector that will be "
190  "in contact.\n"
191  "Also check that this trajectory is defined on the "
192  "time-interval of the phase.\n"
193  "Also check that the trajectory correctly end at the "
194  "placement defined for the contact in the next "
195  "phase.\n"
196  "If this effector was in contact in the previous phase,"
197  "it check that the trajectory start at the previous contact "
198  "placement.\n"
199  "If use_rotation == false, only the translation part of the "
200  "transforms are compared. "))
201  .def("haveJointsTrajectories", &CS::haveJointsTrajectories,
202  "Check that a q trajectory is defined for each phases.\n"
203  "Also check that the time interval of this trajectories matches "
204  "the one of the phase.\n"
205  "and that the trajectories start and end and the correct values "
206  "defined in each phase.")
207  .def("haveJointsDerivativesTrajectories",
208  &CS::haveJointsDerivativesTrajectories,
209  "Check that a dq and ddq trajectories are defined for each "
210  "phases.\n"
211  "Also check that the time interval of this trajectories matches "
212  "the one of the phase.\n"
213  "and that the trajectories start and end and the correct values "
214  "defined in each phase.")
215  .def("haveTorquesTrajectories", &CS::haveTorquesTrajectories,
216  "Check that a joint torque trajectories are defined for each "
217  "phases.\n"
218  "Also check that the time interval of this trajectories matches "
219  "the one of the phase.\n"
220  "and that the trajectories start and end and the correct values "
221  "defined in each phase")
222  .def("haveContactForcesTrajectories",
223  &CS::haveContactForcesTrajectories,
224  "Check that a contact force trajectory exist for each active "
225  "contact.\n"
226  "Also check that the time interval of this trajectories matches "
227  "the one of the phase.\n"
228  "and that the trajectories start and end and the correct values "
229  "defined in each phase.")
230  .def("haveRootTrajectories", &CS::haveRootTrajectories,
231  "check that a root trajectory exist for each contact phases.\n"
232  "Also check that it start and end at the correct time interval.")
233  .def("haveFriction", &CS::haveFriction,
234  "check that all the contact patch used in the sequence have"
235  "a friction coefficient initialized.")
236  .def("haveContactModelDefined", &CS::haveContactModelDefined,
237  "haveContactModelDefined check that all the contact patch have a "
238  "contact_model defined")
239  .def("haveZMPtrajectories", &CS::haveZMPtrajectories,
240  "check that all the contact phases have a ZMP trajectory.")
241  .def("getAllEffectorsInContact", &getAllEffectorsInContactAsList,
242  "return a list of names of all the effectors used to create "
243  "contacts during the sequence")
244  .def("concatenateCtrajectories", &CS::concatenateCtrajectories,
245  "Return a piecewise curve wchich is the concatenation of the m_c "
246  "curves"
247  " for each contact phases in the sequence.")
248  .def("concatenateDCtrajectories", &CS::concatenateDCtrajectories,
249  "Return a piecewise curve wchich is the concatenation of the m_dc "
250  "curves"
251  " for each contact phases in the sequence.")
252  .def("concatenateDDCtrajectories", &CS::concatenateDDCtrajectories,
253  "Return a piecewise curve wchich is the concatenation of the "
254  "m_ddc curves"
255  " for each contact phases in the sequence.")
256  .def("concatenateLtrajectories", &CS::concatenateLtrajectories,
257  "Return a piecewise curve wchich is the concatenation of the m_L "
258  "curves"
259  " for each contact phases in the sequence.")
260  .def("concatenateDLtrajectories", &CS::concatenateDLtrajectories,
261  "Return a piecewise curve wchich is the concatenation of the m_dL "
262  "curves"
263  " for each contact phases in the sequence.")
264  .def("concatenateZMPtrajectories", &CS::concatenateZMPtrajectories,
265  "Return a piecewise curve wchich is the concatenation of the "
266  "m_zmp curves"
267  " for each contact phases in the sequence.")
268  .def("concatenateWrenchTrajectories",
269  &CS::concatenateWrenchTrajectories,
270  "Return a piecewise curve wchich is the concatenation of the "
271  "m_wrench curves"
272  " for each contact phases in the sequence.")
273  .def("concatenateQtrajectories", &CS::concatenateQtrajectories,
274  "Return a piecewise curve wchich is the concatenation of the m_q "
275  "curves"
276  " for each contact phases in the sequence.")
277  .def("concatenateDQtrajectories", &CS::concatenateDQtrajectories,
278  "Return a piecewise curve wchich is the concatenation of the m_dq "
279  "curves"
280  " for each contact phases in the sequence.")
281  .def("concatenateDDQtrajectories", &CS::concatenateDDQtrajectories,
282  "Return a piecewise curve wchich is the concatenation of the "
283  "m_ddq curves"
284  " for each contact phases in the sequence.")
285  .def("concatenateTauTrajectories", &CS::concatenateTauTrajectories,
286  "Return a piecewise curve wchich is the concatenation of the "
287  "m_tau curves"
288  " for each contact phases in the sequence.")
289  .def("concatenateRootTrajectories", &CS::concatenateRootTrajectories,
290  "Return a piecewise curve wchich is the concatenation of the "
291  "m_root curves"
292  " for each contact phases in the sequence.")
293  .def("concatenateEffectorTrajectories",
294  &CS::concatenateEffectorTrajectories, bp::arg("eeName"),
295  "Return a piecewise curve which is the concatenation"
296  "of the effectors trajectories curves for the given effector"
297  "for each contact phases in the sequence.\n"
298  "During the phases where no effector trajectories are defined,"
299  "the trajectory is constant with the value of"
300  "the last phase where it was defined.")
301  .def("concatenateContactForceTrajectories",
302  &CS::concatenateContactForceTrajectories, bp::arg("eeName"),
303  "Return a piecewise curve which"
304  "is the concatenation of the contact forces for the given effector"
305  "for each contact phases in the sequence.\n"
306  "During the phases where no contact forces are defined,"
307  "the trajectory is constant with the value of 0.")
308  .def("concatenateNormalForceTrajectories",
309  &CS::concatenateNormalForceTrajectories, bp::arg("eeName"),
310  "Return a piecewise curve which"
311  "is the concatenation of the contact normal forces for the given "
312  "effector"
313  "for each contact phases in the sequence.\n"
314  "During the phases where no contact normal forces are defined,"
315  "the trajectory is constant with the value of 0.")
316  .def("phaseIdAtTime", &CS::phaseIdAtTime, bp::arg("time"),
317  "return the index of a phase in the sequence such that "
318  "phase.timeInitial <= t < phase.timeFinal \n"
319  "if t equal to the last phase timeFinal, this index is returned.")
320  .def("phaseAtTime", &CS::phaseAtTime, bp::arg("time"),
321  bp::return_internal_reference<>(),
322  "return a phase of the sequence such that "
323  "phase.timeInitial <= t < phase.timeFinal \n"
324  "if t equal to the last phase timeFinal, this index is returned.")
325  .def(bp::self == bp::self)
326  .def(bp::self != bp::self)
327  .def("copy", &copy, "Returns a copy of *this.");
328  }
329 
330  static void expose(const std::string& class_name) {
331  std::string doc = "Contact Sequence of dynamic size";
332  bp::class_<CS>(class_name.c_str(), doc.c_str(), bp::no_init)
334  .def(SerializableVisitor<CS>());
335 
336  bp::class_<ContactPhaseVector>("ContactPhaseVector")
337  .def(bp::vector_indexing_suite<ContactPhaseVector>());
338  }
339 
340  protected:
341  static CS copy(const CS& self) { return CS(self); }
342 
343  // Converts a C++ vector to a python list
344  // Note : lot of overhead, should not be used for large vector and/or
345  // operations called frequently. prefer the direct bindings with
346  // std_vector_strings for this cases.
347  template <class T>
348  static bp::list toPythonList(std::vector<T> vector) {
349  typename std::vector<T>::const_iterator iter;
350  boost::python::list list;
351  for (iter = vector.begin(); iter != vector.end(); ++iter) {
352  list.append(*iter);
353  }
354  return list;
355  }
356 
357  static bp::list getAllEffectorsInContactAsList(CS& self) {
358  return toPythonList<std::string>(self.getAllEffectorsInContact());
359  }
360 };
361 } // namespace python
362 } // namespace multicontact_api
363 
364 #endif // ifndef __multicontact_api_python_scenario_contact_sequence_hpp__
Definition: ellipsoid.hpp:12
static void expose(const std::string &class_name)
Definition: contact-sequence.hpp:330
BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(cs_breakContact_overloads, CS::breakContact, 1, 2) BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(cs_createContact_overloads
static bp::list toPythonList(std::vector< T > vector)
Definition: contact-sequence.hpp:348
static CS copy(const CS &self)
Definition: contact-sequence.hpp:341
CS::ContactPhaseVector ContactPhaseVector
Definition: contact-sequence.hpp:22
static bp::list getAllEffectorsInContactAsList(CS &self)
Definition: contact-sequence.hpp:357