contact-sequence.hpp 17.9 KB
Newer Older
Justin Carpentier's avatar
Justin Carpentier committed
1
2
// Copyright (c) 2015-2018, CNRS
// Authors: Justin Carpentier <jcarpent@laas.fr>
Guilhem Saurel's avatar
Guilhem Saurel committed
3
4
5

#ifndef __multicontact_api_python_scenario_contact_sequence_hpp__
#define __multicontact_api_python_scenario_contact_sequence_hpp__
Justin Carpentier's avatar
Justin Carpentier committed
6

7
#include <pinocchio/fwd.hpp>
Justin Carpentier's avatar
Justin Carpentier committed
8
9
10
#include <eigenpy/eigenpy.hpp>
#include <pinocchio/bindings/python/utils/std-aligned-vector.hpp>

Guilhem Saurel's avatar
Guilhem Saurel committed
11
12
#include "multicontact-api/scenario/contact-sequence.hpp"
#include "multicontact-api/bindings/python/serialization/archive.hpp"
Justin Carpentier's avatar
Justin Carpentier committed
13

Guilhem Saurel's avatar
Guilhem Saurel committed
14
15
namespace multicontact_api {
namespace python {
Guilhem Saurel's avatar
Guilhem Saurel committed
16

Guilhem Saurel's avatar
Guilhem Saurel committed
17
namespace bp = boost::python;
Guilhem Saurel's avatar
Guilhem Saurel committed
18

Guilhem Saurel's avatar
Guilhem Saurel committed
19
template <typename CS>
20
struct ContactSequencePythonVisitor : public bp::def_visitor<ContactSequencePythonVisitor<CS> > {
Guilhem Saurel's avatar
Guilhem Saurel committed
21
  typedef typename CS::ContactPhaseVector ContactPhaseVector;
Guilhem Saurel's avatar
Guilhem Saurel committed
22

23
24
25
26
  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)
27
  BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(cs_haveEffectorTrajectories_overloads, CS::haveEffectorsTrajectories, 0, 2)
28

Guilhem Saurel's avatar
Guilhem Saurel committed
29
30
31
  template <class PyClass>
  void visit(PyClass& cl) const {
    cl.def(bp::init<size_t>(bp::arg("size"), "Default constructor from a given size."))
Pierre Fernbach's avatar
Pierre Fernbach committed
32
        .def(bp::init<>(bp::arg(""), "Default constructor."))
Guilhem Saurel's avatar
Guilhem Saurel committed
33
        .def(bp::init<CS>(bp::args("other"), "Copy contructor."))
34
        .def("size", &CS::size, "Return the size of the contact sequence.")
Pierre Fernbach's avatar
Pierre Fernbach committed
35
36
37
38
39
40
        .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")
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
        .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"))
99
100
        .def("haveTimings", &CS::haveTimings,
             "Check if all the time intervals are defined and consistent"
101
102
103
             "(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")
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
        .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,
132
             cs_haveEffectorTrajectories_overloads(
133
134
                 (bp::args("precision_treshold") = Eigen::NumTraits<typename CS::Scalar>::dummy_precision(),
                  bp::args("use_rotation") = true),
Guilhem Saurel's avatar
format    
Guilhem Saurel committed
135
136
137
138
139
140
                 "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,"
141
142
                 "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. "))
143
        .def("haveJointsTrajectories", &CS::haveJointsTrajectories,
144
145
146
147
148
             "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"
149
150
151
152
153
154
155
156
157
158
             "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.")
159
160
161
        .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.")
162
163
164
        .def("haveFriction", &CS::haveFriction,
             "check that all the contact patch used in the sequence have"
             "a friction coefficient initialized.")
165
166
        .def("haveZMPtrajectories", &CS::haveZMPtrajectories,
             "check that all the contact phases have a ZMP trajectory.")
Guilhem Saurel's avatar
format    
Guilhem Saurel committed
167
        .def("getAllEffectorsInContact", &getAllEffectorsInContactAsList,
168
             "return a list of names of all the effectors used to create contacts during the sequence")
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
        .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.")
187
188
189
        .def("concatenateWrenchTrajectories", &CS::concatenateWrenchTrajectories,
             "Return a piecewise curve wchich is the concatenation of the m_wrench curves"
             " for each contact phases in the sequence.")
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
        .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.")
Guilhem Saurel's avatar
format    
Guilhem Saurel committed
205
        .def("concatenateEffectorTrajectories", &CS::concatenateEffectorTrajectories, bp::arg("eeName"),
206
207
208
209
210
211
             "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.")
Guilhem Saurel's avatar
format    
Guilhem Saurel committed
212
        .def("concatenateContactForceTrajectories", &CS::concatenateContactForceTrajectories, bp::arg("eeName"),
213
214
215
216
217
             "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.")
Guilhem Saurel's avatar
format    
Guilhem Saurel committed
218
        .def("concatenateNormalForceTrajectories", &CS::concatenateNormalForceTrajectories, bp::arg("eeName"),
219
220
221
222
223
             "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.")
Guilhem Saurel's avatar
format    
Guilhem Saurel committed
224
        .def("phaseIdAtTime", &CS::phaseIdAtTime, bp::arg("time"),
225
226
227
             "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.")
Guilhem Saurel's avatar
format    
Guilhem Saurel committed
228
        .def("phaseAtTime", &CS::phaseAtTime, bp::arg("time"), bp::return_internal_reference<>(),
229
230
231
             "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.")
Justin Carpentier's avatar
Justin Carpentier committed
232
233
        .def(bp::self == bp::self)
        .def(bp::self != bp::self)
234
        .def("copy", &copy, "Returns a copy of *this.");
Guilhem Saurel's avatar
Guilhem Saurel committed
235
  }
Guilhem Saurel's avatar
Guilhem Saurel committed
236

Guilhem Saurel's avatar
Guilhem Saurel committed
237
238
239
  static void expose(const std::string& class_name) {
    std::string doc = "Contact Sequence of dynamic size";
    bp::class_<CS>(class_name.c_str(), doc.c_str(), bp::no_init)
Justin Carpentier's avatar
Justin Carpentier committed
240
        .def(ContactSequencePythonVisitor<CS>())
Guilhem Saurel's avatar
Guilhem Saurel committed
241
        .def(SerializableVisitor<CS>());
Guilhem Saurel's avatar
Guilhem Saurel committed
242

Pierre Fernbach's avatar
Pierre Fernbach committed
243
    bp::class_<ContactPhaseVector>("ContactPhaseVector").def(bp::vector_indexing_suite<ContactPhaseVector>());
Justin Carpentier's avatar
Justin Carpentier committed
244
  }
245

Pierre Fernbach's avatar
Pierre Fernbach committed
246
247
 protected:
  static CS copy(const CS& self) { return CS(self); }
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264

  // 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());
  }
Guilhem Saurel's avatar
Guilhem Saurel committed
265
266
267
};
}  // namespace python
}  // namespace multicontact_api
Justin Carpentier's avatar
Justin Carpentier committed
268

Guilhem Saurel's avatar
Guilhem Saurel committed
269
#endif  // ifndef __multicontact_api_python_scenario_contact_sequence_hpp__