Directory: | ./ |
---|---|
File: | include/multicontact-api/bindings/python/scenario/contact-sequence.hpp |
Date: | 2025-03-10 16:17:01 |
Exec | Total | Coverage | |
---|---|---|---|
Lines: | 85 | 94 | 90.4% |
Branches: | 107 | 222 | 48.2% |
Line | Branch | Exec | Source |
---|---|---|---|
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 | |||
11 | #include "multicontact-api/bindings/python/serialization/archive.hpp" | ||
12 | #include "multicontact-api/scenario/contact-sequence.hpp" | ||
13 | |||
14 | namespace multicontact_api { | ||
15 | namespace python { | ||
16 | |||
17 | namespace bp = boost::python; | ||
18 | |||
19 | template <typename CS> | ||
20 | struct ContactSequencePythonVisitor | ||
21 | : public bp::def_visitor<ContactSequencePythonVisitor<CS> > { | ||
22 | typedef typename CS::ContactPhaseVector ContactPhaseVector; | ||
23 | |||
24 | 16 | BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(cs_breakContact_overloads, | |
25 | CS::breakContact, 1, 2) | ||
26 | 14 | BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(cs_createContact_overloads, | |
27 | CS::createContact, 2, 3) | ||
28 | 8 | BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(cs_moveEffectorToPlacement_overloads, | |
29 | CS::moveEffectorToPlacement, 2, 4) | ||
30 | 8 | BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(cs_moveEffectorOf_overloads, | |
31 | CS::moveEffectorOf, 2, 4) | ||
32 | 6 | BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(cs_haveEffectorTrajectories_overloads, | |
33 | CS::haveEffectorsTrajectories, 0, 2) | ||
34 | |||
35 | template <class PyClass> | ||
36 | 3 | void visit(PyClass& cl) const { | |
37 |
2/4✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
|
3 | cl.def(bp::init<size_t>(bp::arg("size"), |
38 | "Default constructor from a given size.")) | ||
39 |
3/6✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3 times.
✗ Branch 8 not taken.
|
6 | .def(bp::init<>(bp::arg(""), "Default constructor.")) |
40 |
3/6✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3 times.
✗ Branch 8 not taken.
|
6 | .def(bp::init<CS>(bp::args("other"), "Copy contructor.")) |
41 | 6 | .def("size", &CS::size, "Return the size of the contact sequence.") | |
42 |
2/4✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
|
6 | .def("resize", &CS::resize, bp::arg("size"), |
43 | "Resize the vector of ContactPhases.") | ||
44 |
2/4✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
|
6 | .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 |
3/6✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3 times.
✗ Branch 8 not taken.
|
3 | .add_property("contactPhases", |
48 | bp::make_getter(&CS::m_contact_phases, | ||
49 | 3 | bp::return_internal_reference<>()), | |
50 | "Vector of Contact Phases contained in the sequence") | ||
51 | 6 | .def("breakContact", &CS::breakContact, | |
52 |
1/2✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
|
3 | cs_breakContact_overloads( |
53 |
4/8✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 3 times.
✗ Branch 11 not taken.
|
6 | (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 |
1/2✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
|
3 | .def("createContact", &CS::createContact, |
72 |
2/4✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
|
6 | cs_createContact_overloads( |
73 |
2/4✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
|
6 | (bp::arg("eeName"), bp::arg("contactPatch"), |
74 |
3/6✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3 times.
✗ Branch 8 not taken.
|
6 | 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 |
1/2✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
|
3 | .def( |
90 | "moveEffectorToPlacement", &CS::moveEffectorToPlacement, | ||
91 |
2/4✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
|
6 | cs_moveEffectorToPlacement_overloads( |
92 |
2/4✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
|
6 | (bp::arg("eeName"), bp::arg("placement"), |
93 |
6/12✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 3 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 3 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 3 times.
✗ Branch 17 not taken.
|
6 | 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 |
1/2✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
|
3 | .def( |
114 | "moveEffectorOf", &CS::moveEffectorOf, | ||
115 |
2/4✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
|
6 | cs_moveEffectorOf_overloads( |
116 |
2/4✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
|
6 | (bp::arg("eeName"), bp::arg("transform"), |
117 |
6/12✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 3 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 3 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 3 times.
✗ Branch 17 not taken.
|
6 | 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 |
1/2✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
|
3 | .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 |
1/2✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
|
3 | .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 |
1/2✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
|
3 | .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 |
1/2✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
|
3 | .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 |
1/2✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
|
3 | .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 |
1/2✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
|
3 | .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 |
1/2✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
|
3 | .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 |
1/2✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
|
3 | .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 |
1/2✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
|
3 | .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 |
1/2✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
|
3 | .def("haveEffectorsTrajectories", &CS::haveEffectorsTrajectories, |
184 |
1/2✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
|
3 | cs_haveEffectorTrajectories_overloads( |
185 |
1/2✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
|
3 | (bp::args("precision_threshold") = |
186 |
1/2✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
|
6 | Eigen::NumTraits<typename CS::Scalar>::dummy_precision(), |
187 |
3/6✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3 times.
✗ Branch 8 not taken.
|
6 | 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 |
1/2✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
|
3 | .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 |
1/2✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
|
3 | .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 |
1/2✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
|
3 | .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 |
1/2✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
|
3 | .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 |
1/2✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
|
3 | .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 |
1/2✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
|
3 | .def("haveFriction", &CS::haveFriction, |
234 | "check that all the contact patch used in the sequence have" | ||
235 | "a friction coefficient initialized.") | ||
236 |
1/2✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
|
3 | .def("haveContactModelDefined", &CS::haveContactModelDefined, |
237 | "haveContactModelDefined check that all the contact patch have a " | ||
238 | "contact_model defined") | ||
239 |
1/2✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
|
3 | .def("haveZMPtrajectories", &CS::haveZMPtrajectories, |
240 | "check that all the contact phases have a ZMP trajectory.") | ||
241 |
2/4✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
|
3 | .def("getAllEffectorsInContact", &getAllEffectorsInContactAsList, |
242 | "return a list of names of all the effectors used to create " | ||
243 | "contacts during the sequence") | ||
244 | 6 | .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 |
1/2✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
|
3 | .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 |
1/2✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
|
3 | .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 |
1/2✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
|
3 | .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 |
1/2✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
|
3 | .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 |
1/2✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
|
3 | .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 |
1/2✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
|
3 | .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 |
1/2✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
|
3 | .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 |
1/2✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
|
3 | .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 |
1/2✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
|
3 | .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 |
1/2✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
|
3 | .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 |
1/2✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
|
3 | .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 |
1/2✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
|
3 | .def("concatenateEffectorTrajectories", |
294 |
1/2✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
|
6 | &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 |
1/2✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
|
3 | .def("concatenateContactForceTrajectories", |
302 |
1/2✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
|
6 | &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 |
1/2✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
|
3 | .def("concatenateNormalForceTrajectories", |
309 |
1/2✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
|
6 | &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 |
2/4✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
|
6 | .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 |
2/4✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
|
3 | .def("phaseAtTime", &CS::phaseAtTime, bp::arg("time"), |
321 | 3 | 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 |
2/4✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
|
3 | .def(bp::self == bp::self) |
326 |
1/2✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
|
6 | .def(bp::self != bp::self) |
327 |
1/2✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
|
3 | .def("copy", ©, "Returns a copy of *this."); |
328 | 3 | } | |
329 | |||
330 | 3 | static void expose(const std::string& class_name) { | |
331 |
1/2✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
|
3 | std::string doc = "Contact Sequence of dynamic size"; |
332 |
1/2✓ Branch 3 taken 3 times.
✗ Branch 4 not taken.
|
3 | bp::class_<CS>(class_name.c_str(), doc.c_str(), bp::no_init) |
333 |
1/2✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
|
3 | .def(ContactSequencePythonVisitor<CS>()) |
334 |
1/2✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
|
3 | .def(SerializableVisitor<CS>()); |
335 | |||
336 |
1/2✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
|
3 | bp::class_<ContactPhaseVector>("ContactPhaseVector") |
337 |
1/2✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
|
3 | .def(bp::vector_indexing_suite<ContactPhaseVector>()); |
338 | 3 | } | |
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__ | ||
365 |