GCC Code Coverage Report

Directory: ./
File: include/multicontact-api/scenario/contact-sequence.hpp
Date: 2025-04-05 01:06:26
Exec Total Coverage
Lines: 339 753 45.0%
Branches: 434 1576 27.5%
Line Branch Exec Source
1 #ifndef __multicontact_api_scenario_contact_sequence_hpp__
2 #define __multicontact_api_scenario_contact_sequence_hpp__
3
4 #include <ndcurves/curve_abc.h>
5 #include <ndcurves/fwd.h>
6 #include <ndcurves/piecewise_curve.h>
7 #include <ndcurves/polynomial.h>
8 #include <ndcurves/se3_curve.h>
9
10 #include <boost/serialization/vector.hpp>
11 #include <vector>
12
13 #include "multicontact-api/scenario/contact-phase.hpp"
14 #include "multicontact-api/scenario/fwd.hpp"
15 #include "multicontact-api/serialization/archive.hpp"
16
17 namespace multicontact_api {
18 namespace scenario {
19
20 template <class _ContactPhase>
21 struct ContactSequenceTpl
22 : public serialization::Serializable<ContactSequenceTpl<_ContactPhase> > {
23 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
24 typedef _ContactPhase ContactPhase;
25 typedef typename ContactPhase::Scalar Scalar;
26 typedef typename ContactPhase::t_strings t_strings;
27 typedef typename ContactPhase::SE3 SE3;
28 typedef std::vector<ContactPhase> ContactPhaseVector;
29
30 typedef ndcurves::pointX_t pointX_t;
31 typedef ndcurves::transform_t transform_t;
32 typedef ndcurves::curve_abc_t curve_t;
33 typedef ndcurves::curve_SE3_t curve_SE3_t;
34 typedef std::shared_ptr<curve_t> curve_ptr;
35 typedef std::shared_ptr<curve_SE3_t> curve_SE3_ptr;
36 typedef ndcurves::piecewise_t piecewise_t;
37 typedef ndcurves::piecewise_SE3_t piecewise_SE3_t;
38 typedef ndcurves::SE3Curve_t SE3Curve_t;
39 typedef ndcurves::polynomial_t polynomial_t;
40
41
1/2
✓ Branch 2 taken 52 times.
✗ Branch 3 not taken.
 52 ContactSequenceTpl(const size_t size = 0) : m_contact_phases(size) {}
42
43 /// \brief Copy contructor
44 3 ContactSequenceTpl(const ContactSequenceTpl& other)
45 3 : m_contact_phases(other.m_contact_phases) {}
46
47 104 size_t size() const { return m_contact_phases.size(); }
48
49 28 bool operator==(const ContactSequenceTpl& other) const {
50 28 return m_contact_phases == other.m_contact_phases;
51 }
52
53 8 bool operator!=(const ContactSequenceTpl& other) const {
54 8 return !(*this == other);
55 }
56
57 11 void resize(const size_t size) { m_contact_phases.resize(size); }
58
59 /* Accessors to the contact Phases */
60
61 /**
62 * @brief append Add the given Phase at the end of the sequence
63 * @param contactPhase the phase to end
64 * @return The id of the phase added in the sequence
65 */
66 146 size_t append(const ContactPhase& contactPhase) {
67 146 m_contact_phases.push_back(contactPhase);
68 146 return m_contact_phases.size() - 1;
69 }
70
71 /**
72 * @brief contactPhases return a Const copy of the contact phase vector in
73 * this sequence. Prefer accessing the contact phases through the
74 * contactPhase(id) as this one create a copy
75 * @return a Const copy of the contact phase vector in this sequence
76 */
77 1 const ContactPhaseVector contactPhases() const { return m_contact_phases; }
78
79 /**
80 * @brief contactPhase return a reference to the contactPhase stored at the
81 * given Id
82 * @param id the desired Id in the contact sequence
83 * @return a reference to the ContactPhase
84 */
85 159 ContactPhase& contactPhase(const size_t id) {
86
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 159 times.
 159 if (id >= m_contact_phases.size())
87 // throw std::invalid_argument("Contact Sequence size is
88 // "+m_contact_phases.size()+" given Id is "+id);
89 ✗ throw std::invalid_argument("Given Id is greater than the vector size");
90 159 return m_contact_phases.at(id);
91 }
92
93 /**
94 * @brief removePhase remove the given contactPhase from the sequence
95 * @param id the Id of the phase to remove
96 */
97 void removePhase(const size_t id) {
98 if (id >= m_contact_phases.size())
99 throw std::invalid_argument("Given Id is greater than the vector size");
100 m_contact_phases.erase(m_contact_phases.begin() + id);
101 }
102
103 /* End Accessors to the contact Phases */
104
105 /* Helpers */
106 /**
107 * @brief breakContact Add a new contactPhase at the end of the current
108 * ContactSequence, The new ContactPhase have the same ContactPatchs as the
109 * last phase of the sequence, with the exeption of the given contact removed.
110 * It copy all the 'final' values of the last phase as 'initial' values of the
111 * new phase. It also set the duration of the previous last phase.
112 * @param eeName the name of the effector to remove from contact
113 * @param phaseDuration if provided, the duration of the previous last phase
114 * of the sequence is set to this value (it is thus the duration BEFORE
115 * breaking the contact)
116 * @throw invalid_argument if the phaseDuration is provided but the last phase
117 * do not have a time-range defined
118 * @throw invalid_argument if eeName is not in contact in the last phase of
119 * the sequence
120 */
121 14 void breakContact(const std::string& eeName,
122 const double phaseDuration = -1) {
123 14 ContactPhase& lastPhase = m_contact_phases.back();
124
3/4
✓ Branch 1 taken 14 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 2 times.
✓ Branch 4 taken 12 times.
 14 if (!lastPhase.isEffectorInContact(eeName))
125
2/4
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
 2 throw std::invalid_argument(
126 "In breakContact : effector is not currently in contact : " + eeName);
127
2/2
✓ Branch 0 taken 8 times.
✓ Branch 1 taken 4 times.
 12 if (phaseDuration > 0) {
128
2/2
✓ Branch 1 taken 2 times.
✓ Branch 2 taken 6 times.
 8 if (lastPhase.timeInitial() < 0) {
129
1/2
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
 2 throw std::invalid_argument(
130 "In breakContact : duration is specified but current phase "
131 "interval in not initialised.");
132 } else {
133
1/2
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
 6 lastPhase.duration(phaseDuration);
134 }
135 }
136
1/2
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
 10 ContactPhase phase;
137 // set initial values from last values of previous phase :
138 10 phase.timeInitial(lastPhase.timeFinal());
139
1/2
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
 10 phase.m_c_init = lastPhase.m_c_final;
140
1/2
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
 10 phase.m_dc_init = lastPhase.m_dc_final;
141
1/2
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
 10 phase.m_ddc_init = lastPhase.m_ddc_final;
142
1/2
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
 10 phase.m_L_init = lastPhase.m_L_final;
143
1/2
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
 10 phase.m_dL_init = lastPhase.m_dL_final;
144
1/2
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
 10 phase.m_q_init = lastPhase.m_q_final;
145
146 // copy contact patchs :
147
4/6
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 7 taken 20 times.
✗ Branch 8 not taken.
✓ Branch 12 taken 20 times.
✓ Branch 13 taken 10 times.
 30 for (std::string name : lastPhase.effectorsInContact()) {
148
2/2
✓ Branch 1 taken 10 times.
✓ Branch 2 taken 10 times.
 20 if (name != eeName) {
149
2/4
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10 times.
✗ Branch 5 not taken.
 10 phase.addContact(name, lastPhase.contactPatch(name));
150 }
151 }
152 // add new phase at the end of the sequence
153
1/2
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
 10 append(phase);
154 20 return;
155 10 }
156
157 /**
158 * @brief createContact Add a new contactPhase at the end of the current
159 * ContactSequence, The new ContactPhase have the same ContactPatchs as the
160 * last phase of the sequence, with the exeption of the given contact added.
161 * @param eeName the name of the effector used to create contact
162 * @param patch the ContactPatch of the new contact
163 * @param phaseDuration if provided, the duration of the previous last phase
164 * of the sequence is set to this value (it is thus the duration BEFORE
165 * creating the contact)
166 * @throw invalid_argument if the phaseDuration is provided but the last phase
167 * do not have a time-range defined
168 * @throw invalid_argument if eeName is already in contact in the last phase
169 * of the sequence
170 */
171 12 void createContact(const std::string& eeName, const ContactPatch& patch,
172 const double phaseDuration = -1) {
173 12 ContactPhase& lastPhase = m_contact_phases.back();
174
3/4
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 2 times.
✓ Branch 4 taken 10 times.
 12 if (lastPhase.isEffectorInContact(eeName))
175
2/4
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
 2 throw std::invalid_argument(
176 "In createContact : effector is already in contact : " + eeName);
177
2/2
✓ Branch 0 taken 8 times.
✓ Branch 1 taken 2 times.
 10 if (phaseDuration > 0) {
178
2/2
✓ Branch 1 taken 2 times.
✓ Branch 2 taken 6 times.
 8 if (lastPhase.timeInitial() < 0) {
179
1/2
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
 2 throw std::invalid_argument(
180 "In createContact : duration is specified but current phase "
181 "interval in not initialised.");
182 } else {
183
1/2
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
 6 lastPhase.duration(phaseDuration);
184 }
185 }
186
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 ContactPhase phase;
187 // set initial values from last values of previous phase :
188 8 phase.timeInitial(lastPhase.timeFinal());
189
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 phase.m_c_init = lastPhase.m_c_final;
190
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 phase.m_dc_init = lastPhase.m_dc_final;
191
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 phase.m_ddc_init = lastPhase.m_ddc_final;
192
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 phase.m_L_init = lastPhase.m_L_final;
193
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 phase.m_dL_init = lastPhase.m_dL_final;
194
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 phase.m_q_init = lastPhase.m_q_final;
195
196 // copy contact patchs :
197
4/6
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 7 taken 8 times.
✗ Branch 8 not taken.
✓ Branch 12 taken 8 times.
✓ Branch 13 taken 8 times.
 16 for (std::string name : lastPhase.effectorsInContact()) {
198
2/4
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
 8 phase.addContact(name, lastPhase.contactPatch(name));
199 }
200 // add new contact to new phase :
201
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 phase.addContact(eeName, patch);
202 // add new phase at the end of the sequence
203
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 append(phase);
204 16 return;
205 8 }
206
207 /**
208 * @brief moveEffectorToPlacement Add two new phases at the end of the current
209 * ContactSequence,
210 * - it break the contact with eeName
211 * - it create the contact with eeName at the given placement.
212 * It copy all the 'final' values of the last phase as 'initial' values of the
213 * new phase. It also set the duration of the previous last phase.
214 * @param eeName the name of the effector used to create contact
215 * @param placement the new placement for the contact of eeName
216 * @param durationBreak the duration of the previous last phase of the
217 * sequence (it is thus the duration BEFORE breaking the contact)
218 * @param durationCreate the duration of the first new ContactPhase
219 * (it is thus the duration BEFORE creating the contact)
220 * @throw invalid_argument if the phaseDuration is provided but the last phase
221 * do not have a time-range defined
222 * @throw invalid_argument if eeName is not in contact in the last phase of
223 * the sequence
224 */
225 4 void moveEffectorToPlacement(const std::string& eeName,
226 const ContactPatch::SE3& placement,
227 const double durationBreak = -1,
228 const double durationCreate = -1) {
229
2/4
✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 4 times.
 4 if (!m_contact_phases.back().isEffectorInContact(eeName))
230 ✗ throw std::invalid_argument(
231 "In moveEffectorToPlacement : effector is not currently in contact "
232 ": " +
233 eeName);
234
2/4
✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 4 times.
✗ Branch 6 not taken.
 4 ContactPatch target(m_contact_phases.back().contactPatch(eeName));
235
1/2
✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
 4 target.placement() = placement;
236
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 4 breakContact(eeName, durationBreak);
237 // copy all "init" value to "final" for the current last phase :
238 4 ContactPhase& lastPhase = m_contact_phases.back();
239
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 4 lastPhase.m_c_final = lastPhase.m_c_init;
240
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 4 lastPhase.m_dc_final = lastPhase.m_dc_init;
241
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 4 lastPhase.m_ddc_final = lastPhase.m_ddc_init;
242
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 4 lastPhase.m_L_final = lastPhase.m_L_init;
243
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 4 lastPhase.m_dL_final = lastPhase.m_dL_init;
244
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 4 lastPhase.m_q_final = lastPhase.m_q_init;
245
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 4 createContact(eeName, target, durationCreate);
246 8 return;
247 4 }
248
249 /**
250 * @brief moveEffectorOf similar to moveEffectorToPlacement
251 * exept that the new placement is defined from the previous placement and a
252 * given transform applied.
253 * @param eeName the name of the effector used to create contact
254 * @param transform the transform applied to the placement of the contact in
255 * the last phase of the sequence
256 * @param durationBreak the duration of the previous last phase of the
257 * sequence (it is thus the duration BEFORE breaking the contact)
258 * @param durationCreate the duration of the first new ContactPhase
259 * (it is thus the duration BEFORE creating the contact)
260 * @throw invalid_argument if the phaseDuration is provided but the last phase
261 * do not have a time-range defined
262 * @throw invalid_argument if eeName is not in contact in the last phase of
263 * the sequence
264 */
265 2 void moveEffectorOf(const std::string& eeName,
266 const ContactPatch::SE3& transform,
267 const double durationBreak = -1,
268 const double durationCreate = -1) {
269
2/4
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 2 times.
 2 if (!m_contact_phases.back().isEffectorInContact(eeName))
270 ✗ throw std::invalid_argument(
271 "In moveEffectorToPlacement : effector is not currently in contact "
272 ": " +
273 eeName);
274
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 ContactPatch::SE3 previous(
275
1/2
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
 2 m_contact_phases.back().contactPatch(eeName).placement());
276
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 ContactPatch::SE3 target = transform.act(previous);
277
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 return moveEffectorToPlacement(eeName, target, durationBreak,
278 4 durationCreate);
279 }
280
281 /**
282 * @brief haveTimings Check if all the time intervals are defined and
283 * consistent (ie. the time always increase and the final time of one phase is
284 * equal to the initial one of the newt phase)
285 * @return true if the sequence is consistent, false otherwise
286 */
287 19 bool haveTimings() const {
288 19 double current_t = m_contact_phases.front().timeInitial();
289
2/2
✓ Branch 0 taken 2 times.
✓ Branch 1 taken 17 times.
 19 if (current_t < 0.) {
290 2 std::cout << "Initial time is negative." << std::endl;
291 2 return false;
292 }
293
2/2
✓ Branch 1 taken 37 times.
✓ Branch 2 taken 13 times.
 50 for (size_t i = 0; i < size(); ++i) {
294 37 const ContactPhase& phase = m_contact_phases.at(i);
295
2/2
✓ Branch 1 taken 4 times.
✓ Branch 2 taken 33 times.
 37 if (phase.timeInitial() != current_t) {
296 4 std::cout << "For phase " << i
297 4 << " initial time is not equal to previous final time."
298 4 << std::endl;
299 4 return false;
300 }
301
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 33 times.
 33 if (phase.timeInitial() > phase.timeFinal()) {
302 ✗ std::cout << "For phase " << i << " final time is before initial time."
303 ✗ << std::endl;
304 ✗ return false;
305 }
306 33 current_t = phase.timeFinal();
307 }
308 13 return true;
309 }
310
311 /**
312 * @brief haveConsistentContacts check that there is always one contact change
313 * between adjacent phases in the sequence and that there isn't any phase
314 * without any contact.
315 * @return
316 */
317 ✗ bool haveConsistentContacts() const {
318 size_t variations;
319 ✗ if (m_contact_phases.front().numContacts() == 0) {
320 // FIXME : may want to test this in a separate method in the future
321 ✗ std::cout << "Phase without any contact at id : 0" << std::endl;
322 ✗ return false;
323 }
324 ✗ for (size_t i = 1; i < m_contact_phases.size(); ++i) {
325 ✗ variations = m_contact_phases.at(i - 1)
326 ✗ .getContactsBroken(m_contact_phases.at(i))
327 ✗ .size() +
328 ✗ m_contact_phases.at(i - 1)
329 ✗ .getContactsCreated(m_contact_phases.at(i))
330 ✗ .size();
331 ✗ if (variations > 1) {
332 ✗ std::cout << "Several contact changes between adjacents phases at id : "
333 ✗ << i << std::endl;
334 ✗ return false;
335 }
336 ✗ if (m_contact_phases.at(i - 1)
337 ✗ .getContactsRepositioned(m_contact_phases.at(i))
338 ✗ .size() > 0) {
339 std::cout
340 ✗ << "Contact repositionning without intermediate phase at id : " << i
341 ✗ << std::endl;
342 ✗ return false;
343 }
344 ✗ if (m_contact_phases.at(i).numContacts() == 0) {
345 // FIXME : may want to test this in a separate method in the future
346 ✗ std::cout << "Phase without any contact at id : " << i << std::endl;
347 ✗ return false;
348 }
349 ✗ if (variations == 0) {
350 ✗ std::cout << "No contact change between adjacents phases at id : " << i
351 ✗ << std::endl;
352 ✗ return false;
353 }
354 }
355 ✗ return true;
356 }
357
358 /**
359 * @brief haveCOMvalues Check that the initial and final CoM position values
360 * are defined for all phases Also check that the initial values of one phase
361 * correspond to the final values of the previous ones.
362 * @return
363 */
364 2 bool haveCOMvalues() const {
365
2/4
✓ Branch 3 taken 2 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✓ Branch 6 taken 2 times.
 2 if (m_contact_phases.front().m_c_init.isZero()) {
366 ✗ std::cout << "Initial CoM position not defined." << std::endl;
367 ✗ return false;
368 }
369
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 2 times.
 2 for (size_t i = 1; i < m_contact_phases.size(); ++i) {
370 ✗ if (m_contact_phases.at(i).m_c_init.isZero()) {
371 ✗ std::cout << "Intermediate CoM position not defined for phase : " << i
372 ✗ << std::endl;
373 ✗ return false;
374 }
375 ✗ if (m_contact_phases.at(i).m_c_init !=
376 ✗ m_contact_phases.at(i - 1).m_c_final) {
377 ✗ std::cout << "Init CoM position do not match final CoM of previous "
378 "phase for id : "
379 ✗ << i << std::endl;
380 ✗ return false;
381 }
382 ✗ if (!m_contact_phases.at(i).m_dc_init.isZero()) {
383 ✗ if (m_contact_phases.at(i).m_dc_init !=
384 ✗ m_contact_phases.at(i - 1).m_dc_final) {
385 ✗ std::cout << "Init CoM velocity do not match final velocity of "
386 "previous phase for id : "
387 ✗ << i << std::endl;
388 ✗ return false;
389 }
390 }
391 ✗ if (!m_contact_phases.at(i).m_ddc_init.isZero()) {
392 ✗ if (m_contact_phases.at(i).m_ddc_init !=
393 ✗ m_contact_phases.at(i - 1).m_ddc_final) {
394 ✗ std::cout << "Init CoM acceleration do not match final acceleration "
395 "of previous phase for id : "
396 ✗ << i << std::endl;
397 ✗ return false;
398 }
399 }
400 }
401
2/4
✓ Branch 3 taken 2 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✓ Branch 6 taken 2 times.
 2 if (m_contact_phases.back().m_c_final.isZero()) {
402 ✗ std::cout << "Final CoM position not defined." << std::endl;
403 ✗ return false;
404 }
405 2 return true;
406 }
407
408 /**
409 * @brief haveAMvalues Check that the initial and final AM values are defined
410 * for all phases Also check that the initial values of one phase correspond
411 * to the final values of the previous ones.
412 * @return
413 */
414 2 bool haveAMvalues() const {
415
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 2 times.
 2 for (size_t i = 1; i < m_contact_phases.size(); ++i) {
416 ✗ if (m_contact_phases.at(i).m_L_init !=
417 ✗ m_contact_phases.at(i - 1).m_L_final) {
418 ✗ std::cout << "Init AM value do not match final value of previous phase "
419 "for id : "
420 ✗ << i << std::endl;
421 ✗ return false;
422 }
423 ✗ if (m_contact_phases.at(i).m_dL_init !=
424 ✗ m_contact_phases.at(i - 1).m_dL_final) {
425 ✗ std::cout << "Init AM derivative do not match final AM derivative of "
426 "previous phase for id : "
427 ✗ << i << std::endl;
428 ✗ return false;
429 }
430 }
431 2 return true;
432 }
433
434 /**
435 * @brief haveCentroidalValues Check that the initial and final CoM position
436 * and AM values are defined for all phases Also check that the initial values
437 * of one phase correspond to the final values of the previous ones.
438 * @return
439 */
440 ✗ bool haveCentroidalValues() const {
441 ✗ return haveAMvalues() && haveCOMvalues();
442 }
443
444 /**
445 * @brief haveConfigurationsValues Check that the initial and final
446 * configuration are defined for all phases Also check that the initial values
447 * of one phase correspond to the final values of the previous ones.
448 * @return
449 */
450 ✗ bool haveConfigurationsValues() const {
451 ✗ if (m_contact_phases.front().m_q_init.isZero()) {
452 ✗ std::cout << "Initial configuration not defined." << std::endl;
453 ✗ return false;
454 }
455 ✗ for (size_t i = 1; i < m_contact_phases.size(); ++i) {
456 ✗ if (m_contact_phases.at(i).m_q_init.isZero()) {
457 ✗ std::cout << "Intermediate configuration not defined for phase : " << i
458 ✗ << std::endl;
459 ✗ return false;
460 }
461 ✗ if (m_contact_phases.at(i).m_q_init !=
462 ✗ m_contact_phases.at(i - 1).m_q_final) {
463 ✗ std::cout << "Init configuration do not match final configuration of "
464 "previous phase for id : "
465 ✗ << i << std::endl;
466 ✗ return false;
467 }
468 }
469 ✗ if (m_contact_phases.back().m_q_final.isZero()) {
470 ✗ std::cout << "Final configuration not defined." << std::endl;
471 ✗ return false;
472 }
473 ✗ return true;
474 }
475
476 /**
477 * @brief haveCOMtrajectories check that a c, dc and ddc trajectories are
478 * defined for each phases Also check that the time interval of this
479 * trajectories matches the one of the phase and that the trajectories start
480 * and end and the correct values defined in each phase
481 * @return
482 */
483 2 bool haveCOMtrajectories() const {
484
3/6
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
✓ Branch 7 taken 2 times.
 2 if (!(haveTimings() && haveCOMvalues())) return false;
485 2 size_t i = 0;
486
2/2
✓ Branch 4 taken 2 times.
✓ Branch 5 taken 1 times.
 3 for (const ContactPhase& phase : m_contact_phases) {
487
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 2 times.
 2 if (!phase.m_c) {
488 ✗ std::cout << "CoM position trajectory not defined for phase : " << i
489 ✗ << std::endl;
490 1 return false;
491 }
492
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 2 times.
 2 if (!phase.m_dc) {
493 ✗ std::cout << "CoM velocity trajectory not defined for phase : " << i
494 ✗ << std::endl;
495 ✗ return false;
496 }
497
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 2 times.
 2 if (!phase.m_ddc) {
498 ✗ std::cout << "CoM acceleration trajectory not defined for phase : " << i
499 ✗ << std::endl;
500 ✗ return false;
501 }
502
2/4
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 2 times.
 2 if (phase.m_c->dim() != 3) {
503 ✗ std::cout << "CoM trajectory is not of dimension 3 for phase : " << i
504 ✗ << std::endl;
505 ✗ return false;
506 }
507
3/4
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 1 times.
✓ Branch 5 taken 1 times.
 2 if (phase.m_dc->dim() != 3) {
508 std::cout
509
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 << "CoM velocity trajectory is not of dimension 3 for phase : " << i
510
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 << std::endl;
511 1 return false;
512 }
513
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 1 times.
 1 if (phase.m_ddc->dim() != 3) {
514 std::cout
515 ✗ << "CoM acceleration trajectory is not of dimension 3 for phase : "
516 ✗ << i << std::endl;
517 ✗ return false;
518 }
519
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✗ Branch 5 not taken.
✓ Branch 6 taken 1 times.
 1 if (phase.m_c->min() != phase.timeInitial()) {
520 ✗ std::cout << "CoM trajectory do not start at t_init for phase : " << i
521 ✗ << std::endl;
522 ✗ return false;
523 }
524
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✗ Branch 5 not taken.
✓ Branch 6 taken 1 times.
 1 if (phase.m_dc->min() != phase.timeInitial()) {
525 std::cout
526 ✗ << "CoM velocity trajectory do not start at t_init for phase : "
527 ✗ << i << std::endl;
528 ✗ return false;
529 }
530
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✗ Branch 5 not taken.
✓ Branch 6 taken 1 times.
 1 if (phase.m_ddc->min() != phase.timeInitial()) {
531 std::cout
532 ✗ << "CoM acceleration trajectory do not start at t_init for phase : "
533 ✗ << i << std::endl;
534 ✗ return false;
535 }
536
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✗ Branch 5 not taken.
✓ Branch 6 taken 1 times.
 1 if (phase.m_c->max() != phase.timeFinal()) {
537 ✗ std::cout << "CoM trajectory do not end at t_final for phase : " << i
538 ✗ << std::endl;
539 ✗ return false;
540 }
541
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✗ Branch 5 not taken.
✓ Branch 6 taken 1 times.
 1 if (phase.m_dc->max() != phase.timeFinal()) {
542 std::cout
543 ✗ << "CoM velocity trajectory do not end at t_final for phase : " << i
544 ✗ << std::endl;
545 ✗ return false;
546 }
547
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✗ Branch 5 not taken.
✓ Branch 6 taken 1 times.
 1 if (phase.m_ddc->max() != phase.timeFinal()) {
548 std::cout
549 ✗ << "CoM acceleration trajectory do not end at t_final for phase : "
550 ✗ << i << std::endl;
551 ✗ return false;
552 }
553
4/8
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✓ Branch 14 taken 1 times.
 1 if (!(*phase.m_c)(phase.m_c->min()).isApprox(phase.m_c_init)) {
554 ✗ std::cout << "CoM trajectory do not start at c_init for phase : " << i
555 ✗ << std::endl;
556 ✗ return false;
557 }
558
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 1 times.
 1 if (phase.m_dc_init.isZero()) {
559 ✗ if (!(*phase.m_dc)(phase.m_dc->min()).isZero()) {
560 std::cout
561 ✗ << "CoM velocity trajectory do not start at dc_init for phase : "
562 ✗ << i << std::endl;
563 ✗ return false;
564 }
565 }
566 // FIXME : isApprox do not work when values close to 0
567 1 else if (!(*phase.m_dc)(phase.m_dc->min())
568
4/8
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✓ Branch 11 taken 1 times.
 1 .isApprox(phase.m_dc_init, 1e-6)) {
569 std::cout
570 ✗ << "CoM velocity trajectory do not start at dc_init for phase : "
571 ✗ << i << std::endl;
572 ✗ return false;
573 }
574
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 1 times.
 1 if (phase.m_ddc_init.isZero()) {
575 ✗ if (!(*phase.m_ddc)(phase.m_ddc->min()).isZero()) {
576 ✗ std::cout << "CoM acceleration trajectory do not start at ddc_init "
577 "for phase : "
578 ✗ << i << std::endl;
579 ✗ return false;
580 }
581 1 } else if (!(*phase.m_ddc)(phase.m_ddc->min())
582
4/8
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✓ Branch 11 taken 1 times.
 1 .isApprox(phase.m_ddc_init, 1e-6)) {
583 ✗ std::cout << "CoM acceleration trajectory do not start at ddc_init for "
584 "phase : "
585 ✗ << i << std::endl;
586 ✗ return false;
587 }
588
4/8
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✓ Branch 14 taken 1 times.
 1 if (!(*phase.m_c)(phase.m_c->max()).isApprox(phase.m_c_final)) {
589 ✗ std::cout << "CoM trajectory do not end at c_final for phase : " << i
590 ✗ << std::endl;
591 ✗ return false;
592 }
593
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 1 times.
 1 if (phase.m_dc_final.isZero()) {
594 ✗ if (!(*phase.m_dc)(phase.m_dc->max()).isZero()) {
595 std::cout
596 ✗ << "CoM velocity trajectory do not end at dc_final for phase : "
597 ✗ << i << std::endl;
598 ✗ return false;
599 }
600 1 } else if (!(*phase.m_dc)(phase.m_dc->max())
601
4/8
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✓ Branch 11 taken 1 times.
 1 .isApprox(phase.m_dc_final, 1e-6)) {
602 std::cout
603 ✗ << "CoM velocity trajectory do not end at dc_final for phase : "
604 ✗ << i << std::endl;
605 ✗ return false;
606 }
607
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 1 times.
 1 if (phase.m_ddc_final.isZero()) {
608 ✗ if (!(*phase.m_ddc)(phase.m_ddc->max()).isZero()) {
609 ✗ std::cout << "CoM acceleration trajectory do not end at ddc_final "
610 "for phase : "
611 ✗ << i << std::endl;
612 ✗ return false;
613 }
614 1 } else if (!(*phase.m_ddc)(phase.m_ddc->max())
615
4/8
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✓ Branch 11 taken 1 times.
 1 .isApprox(phase.m_ddc_final, 1e-6)) {
616 ✗ std::cout << "CoM acceleration trajectory do not end at ddc_final for "
617 "phase : "
618 ✗ << i << std::endl;
619 ✗ return false;
620 }
621 1 ++i;
622 }
623 1 return true;
624 }
625
626 /**
627 * @brief haveAMtrajectories check that a L and dL trajectories are defined
628 * for each phases Also check that the time interval of this trajectories
629 * matches the one of the phase and that the trajectories start and end and
630 * the correct values defined in each phase
631 * @return
632 */
633 2 bool haveAMtrajectories() const {
634
3/6
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
✓ Branch 7 taken 2 times.
 2 if (!(haveTimings() && haveAMvalues())) return false;
635 2 size_t i = 0;
636
2/2
✓ Branch 4 taken 2 times.
✓ Branch 5 taken 1 times.
 3 for (const ContactPhase& phase : m_contact_phases) {
637
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 2 times.
 2 if (!phase.m_L) {
638 ✗ std::cout << "AM position trajectory not defined for phase : " << i
639 ✗ << std::endl;
640 1 return false;
641 }
642
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 2 times.
 2 if (!phase.m_dL) {
643 ✗ std::cout << "AM velocity trajectory not defined for phase : " << i
644 ✗ << std::endl;
645 ✗ return false;
646 }
647
3/4
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 1 times.
✓ Branch 5 taken 1 times.
 2 if (phase.m_L->dim() != 3) {
648
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 std::cout << "AM trajectory is not of dimension 3 for phase : " << i
649
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 << std::endl;
650 1 return false;
651 }
652
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 1 times.
 1 if (phase.m_dL->dim() != 3) {
653 std::cout
654 ✗ << "AM derivative trajectory is not of dimension 3 for phase : "
655 ✗ << i << std::endl;
656 ✗ return false;
657 }
658
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✗ Branch 5 not taken.
✓ Branch 6 taken 1 times.
 1 if (phase.m_L->min() != phase.timeInitial()) {
659 ✗ std::cout << "AM trajectory do not start at t_init for phase : " << i
660 ✗ << std::endl;
661 ✗ return false;
662 }
663
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✗ Branch 5 not taken.
✓ Branch 6 taken 1 times.
 1 if (phase.m_dL->min() != phase.timeInitial()) {
664 std::cout
665 ✗ << "AM derivative trajectory do not start at t_init for phase : "
666 ✗ << i << std::endl;
667 ✗ return false;
668 }
669
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✗ Branch 5 not taken.
✓ Branch 6 taken 1 times.
 1 if (phase.m_L->max() != phase.timeFinal()) {
670 ✗ std::cout << "AM trajectory do not end at t_final for phase : " << i
671 ✗ << std::endl;
672 ✗ return false;
673 }
674
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✗ Branch 5 not taken.
✓ Branch 6 taken 1 times.
 1 if (phase.m_dL->max() != phase.timeFinal()) {
675 std::cout
676 ✗ << "AM derivative trajectory do not end at t_final for phase : "
677 ✗ << i << std::endl;
678 ✗ return false;
679 }
680
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 1 times.
 1 if (phase.m_L_init.isZero()) {
681 ✗ if (!(*phase.m_L)(phase.m_L->min()).isZero()) {
682 ✗ std::cout << "AM trajectory do not start at L_init for phase : " << i
683 ✗ << std::endl;
684 ✗ return false;
685 }
686
4/8
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✓ Branch 14 taken 1 times.
 1 } else if (!(*phase.m_L)(phase.m_L->min()).isApprox(phase.m_L_init)) {
687 ✗ std::cout << "AM trajectory do not start at L_init for phase : " << i
688 ✗ << std::endl;
689 ✗ return false;
690 }
691
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 1 times.
 1 if (phase.m_dL_init.isZero()) {
692 ✗ if (!(*phase.m_dL)(phase.m_dL->min()).isZero()) {
693 std::cout
694 ✗ << "AM derivative trajectory do not start at dL_init for phase : "
695 ✗ << i << std::endl;
696 ✗ return false;
697 }
698 1 } else if (!(*phase.m_dL)(phase.m_dL->min())
699
4/8
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✓ Branch 11 taken 1 times.
 1 .isApprox(phase.m_dL_init, 1e-6)) {
700 std::cout
701 ✗ << "AM derivative trajectory do not start at dL_init for phase : "
702 ✗ << i << std::endl;
703 ✗ return false;
704 }
705
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 1 times.
 1 if (phase.m_L_final.isZero()) {
706 ✗ if (!(*phase.m_L)(phase.m_L->max()).isZero()) {
707 ✗ std::cout << "AM trajectory do not end at L_final for phase : " << i
708 ✗ << std::endl;
709 ✗ return false;
710 }
711 1 } else if (!(*phase.m_L)(phase.m_L->max())
712
4/8
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✓ Branch 11 taken 1 times.
 1 .isApprox(phase.m_L_final, 1e-6)) {
713 ✗ std::cout << "AM trajectory do not end at L_final for phase : " << i
714 ✗ << std::endl;
715 ✗ return false;
716 }
717
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 1 times.
 1 if (phase.m_dL_final.isZero()) {
718 ✗ if (!(*phase.m_dL)(phase.m_dL->max()).isZero()) {
719 std::cout
720 ✗ << "AM derivative trajectory do not end at dL_final for phase : "
721 ✗ << i << std::endl;
722 ✗ return false;
723 }
724
4/8
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✓ Branch 14 taken 1 times.
 1 } else if (!(*phase.m_dL)(phase.m_dL->max()).isApprox(phase.m_dL_final)) {
725 std::cout
726 ✗ << "AM derivative trajectory do not end at dL_final for phase : "
727 ✗ << i << std::endl;
728 ✗ return false;
729 }
730 1 ++i;
731 }
732 1 return true;
733 }
734
735 /**
736 * @brief haveCentroidalTrajectories check that all centroidal trajectories
737 * are defined for each phases Also check that the time interval of this
738 * trajectories matches the one of the phase and that the trajectories start
739 * and end and the correct values defined in each phase
740 * @return
741 */
742 ✗ bool haveCentroidalTrajectories() const {
743 ✗ return haveAMtrajectories() && haveCOMtrajectories();
744 }
745
746 /**
747 * @brief haveEffectorsTrajectories check that for each phase preceeding a
748 * contact creation, an SE3 trajectory is defined for the effector that will
749 * be in contact. Also check that this trajectory is defined on the
750 * time-interval of the phase. Also check that the trajectory correctly end at
751 * the placement defined for the contact in the next phase. If this effector
752 * was in contact in the previous phase, it check that the trajectory start at
753 * the previous contact placement.
754 * @return
755 */
756 7 bool haveEffectorsTrajectories(
757 const Scalar prec = Eigen::NumTraits<Scalar>::dummy_precision(),
758 const bool use_rotation = true) const {
759
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 7 times.
 7 if (!haveTimings()) return false;
760
2/2
✓ Branch 1 taken 14 times.
✓ Branch 2 taken 4 times.
 18 for (size_t i = 0; i < m_contact_phases.size() - 1; ++i) {
761
10/12
✓ Branch 1 taken 14 times.
✗ Branch 2 not taken.
✓ Branch 7 taken 7 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 4 times.
✓ Branch 11 taken 3 times.
✓ Branch 13 taken 4 times.
✓ Branch 14 taken 3 times.
✓ Branch 17 taken 7 times.
✓ Branch 18 taken 11 times.
✓ Branch 20 taken 11 times.
✓ Branch 21 taken 3 times.
 45 for (std::string eeName : m_contact_phases.at(i).getContactsCreated(
762
2/4
✓ Branch 1 taken 14 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 14 times.
✗ Branch 5 not taken.
 14 m_contact_phases.at(i + 1))) {
763
3/6
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✓ Branch 7 taken 7 times.
 7 if (!m_contact_phases.at(i).effectorHaveAtrajectory(eeName)) {
764 std::cout << "No end effector trajectory for " << eeName
765 ✗ << " at phase " << i << " but it is in contact at phase "
766 ✗ << i + 1 << std::endl;
767 ✗ return false;
768 }
769 7 const typename ContactPhase::curve_SE3_ptr traj =
770
3/6
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 7 times.
✗ Branch 8 not taken.
 7 m_contact_phases.at(i).effectorTrajectories().at(eeName);
771
3/6
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✗ Branch 8 not taken.
✓ Branch 9 taken 7 times.
 7 if (traj->min() != m_contact_phases.at(i).timeInitial()) {
772 std::cout << "Effector trajectory for " << eeName
773 ✗ << " do not start at t_init in phase " << i << std::endl;
774 ✗ return false;
775 }
776
3/6
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✗ Branch 8 not taken.
✓ Branch 9 taken 7 times.
 7 if (traj->max() != m_contact_phases.at(i).timeFinal()) {
777 std::cout << "Effector trajectory for " << eeName
778 ✗ << " do not end at t_final in phase " << i << std::endl;
779 ✗ return false;
780 }
781
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 7 ContactPatch::SE3 pTrajMax =
782
2/4
✓ Branch 3 taken 7 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 7 times.
✗ Branch 7 not taken.
 7 ContactPatch::SE3((*traj)(traj->max()).matrix());
783
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 7 ContactPatch::SE3 pPhaseMax =
784
3/6
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 7 times.
✗ Branch 8 not taken.
 14 m_contact_phases.at(i + 1).contactPatches().at(eeName).placement();
785 13 if ((use_rotation &&
786 !(pTrajMax.toHomogeneousMatrix() - pPhaseMax.toHomogeneousMatrix())
787
8/12
✓ Branch 0 taken 6 times.
✓ Branch 1 taken 1 times.
✓ Branch 3 taken 6 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 6 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 6 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 6 times.
✗ Branch 13 not taken.
✓ Branch 14 taken 3 times.
✓ Branch 15 taken 3 times.
 11 .isMuchSmallerThan(1.0, prec)) ||
788
2/4
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
 4 !(pTrajMax.translation() - pPhaseMax.translation())
789
5/8
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✓ Branch 7 taken 4 times.
✓ Branch 8 taken 3 times.
✓ Branch 9 taken 4 times.
 11 .isMuchSmallerThan(1.0, prec)) {
790 std::cout << "Effector trajectory for " << eeName
791
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 << " do not end at it's contact placement in the next "
792 "phase, for phase "
793
2/4
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
 3 << i << std::endl;
794
2/4
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
 3 std::cout << "Last point : " << std::endl
795
2/4
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
 3 << pTrajMax << std::endl
796
2/4
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
 3 << "Next contact : " << std::endl
797
2/4
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
 3 << pPhaseMax << std::endl;
798 3 return false;
799 }
800
5/10
✓ Branch 0 taken 4 times.
✗ Branch 1 not taken.
✓ Branch 3 taken 4 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 4 times.
✗ Branch 7 not taken.
✓ Branch 8 taken 4 times.
✗ Branch 9 not taken.
✓ Branch 10 taken 4 times.
✗ Branch 11 not taken.
 4 if (i > 0 && m_contact_phases.at(i - 1).isEffectorInContact(eeName)) {
801
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 4 ContactPatch::SE3 pTrajMin =
802
2/4
✓ Branch 3 taken 4 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 4 times.
✗ Branch 7 not taken.
 4 ContactPatch::SE3((*traj)(traj->min()).matrix());
803
2/4
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
 8 ContactPatch::SE3 pPhaseMin = m_contact_phases.at(i - 1)
804 .contactPatches()
805
2/4
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
 8 .at(eeName)
806 4 .placement();
807 7 if ((use_rotation && !(pTrajMin.toHomogeneousMatrix() -
808 pPhaseMin.toHomogeneousMatrix())
809
7/12
✓ Branch 0 taken 3 times.
✓ Branch 1 taken 1 times.
✓ Branch 3 taken 3 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 3 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 3 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 3 times.
✗ Branch 13 not taken.
✓ Branch 14 taken 3 times.
✗ Branch 15 not taken.
 8 .isMuchSmallerThan(1.0, prec)) ||
810
2/4
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
 4 !(pTrajMin.translation() - pPhaseMin.translation())
811
4/8
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✓ Branch 7 taken 4 times.
✗ Branch 8 not taken.
✓ Branch 9 taken 4 times.
 8 .isMuchSmallerThan(1.0, prec)) {
812 std::cout << "Effector trajectory for " << eeName
813 ✗ << " do not start at it's contact placement in the "
814 "previous phase, for phase "
815 ✗ << i << std::endl;
816 ✗ std::cout << "First point : " << std::endl
817 ✗ << pTrajMin << std::endl
818 ✗ << "Previous contact : " << std::endl
819 ✗ << pPhaseMin << std::endl;
820 ✗ return false;
821 }
822 }
823 }
824 }
825 4 return true;
826 }
827
828 /**
829 * @brief haveJointsTrajectories Check that a q trajectory is defined for each
830 * phases Also check that the time interval of this trajectories matches the
831 * one of the phase and that the trajectories start and end and the correct
832 * values defined in each phase
833 * @return
834 */
835 ✗ bool haveJointsTrajectories() const {
836 ✗ if (!(haveTimings() && haveConfigurationsValues())) return false;
837 ✗ size_t i = 0;
838 ✗ for (const ContactPhase& phase : m_contact_phases) {
839 ✗ if (!phase.m_q) {
840 ✗ std::cout << "joint position trajectory not defined for phase : " << i
841 ✗ << std::endl;
842 ✗ return false;
843 }
844 ✗ if (phase.m_q->min() != phase.timeInitial()) {
845 ✗ std::cout << "joint trajectory do not start at t_init for phase : " << i
846 ✗ << std::endl;
847 ✗ return false;
848 }
849 ✗ if (phase.m_q->max() != phase.timeFinal()) {
850 ✗ std::cout << "joint trajectory do not end at t_final for phase : " << i
851 ✗ << std::endl;
852 ✗ return false;
853 }
854 ✗ if (!(*phase.m_q)(phase.m_q->min()).isApprox(phase.m_q_init)) {
855 ✗ std::cout << "joint trajectory do not start at q_init for phase : " << i
856 ✗ << std::endl;
857 ✗ return false;
858 }
859 ✗ if (!(*phase.m_q)(phase.m_q->max()).isApprox(phase.m_q_final)) {
860 ✗ std::cout << "joint trajectory do not end at q_final for phase : " << i
861 ✗ << std::endl;
862 ✗ return false;
863 }
864 ✗ ++i;
865 }
866 ✗ return true;
867 }
868
869 /**
870 * @brief haveJointsDerivativesTrajectories Check that a dq and ddq
871 * trajectories are defined for each phases Also check that the time interval
872 * of this trajectories matches the one of the phase and that the trajectories
873 * start and end and the correct values defined in each phase
874 * @return
875 */
876 ✗ bool haveJointsDerivativesTrajectories() const {
877 ✗ if (!(haveTimings())) return false;
878 ✗ size_t i = 0;
879 ✗ for (const ContactPhase& phase : m_contact_phases) {
880 ✗ if (!phase.m_dq) {
881 ✗ std::cout << "joint velocity trajectory not defined for phase : " << i
882 ✗ << std::endl;
883 ✗ return false;
884 }
885 ✗ if (!phase.m_ddq) {
886 ✗ std::cout << "joint acceleration trajectory not defined for phase : "
887 ✗ << i << std::endl;
888 ✗ return false;
889 }
890 ✗ if (phase.m_dq->min() != phase.timeInitial()) {
891 std::cout
892 ✗ << "joint velocity trajectory do not start at t_init for phase : "
893 ✗ << i << std::endl;
894 ✗ return false;
895 }
896 ✗ if (phase.m_ddq->min() != phase.timeInitial()) {
897 ✗ std::cout << "joint acceleration trajectory do not start at t_init for "
898 "phase : "
899 ✗ << i << std::endl;
900 ✗ return false;
901 }
902 ✗ if (phase.m_dq->max() != phase.timeFinal()) {
903 std::cout
904 ✗ << "joint velocity trajectory do not end at t_final for phase : "
905 ✗ << i << std::endl;
906 ✗ return false;
907 }
908 ✗ if ((phase.m_ddq->max() != phase.timeFinal()) &&
909 ✗ i < size() - 1) { // not required for the last phase
910 ✗ std::cout << "joint acceleration trajectory do not end at t_final for "
911 "phase : "
912 ✗ << i << std::endl;
913 ✗ return false;
914 }
915 ✗ ++i;
916 }
917 ✗ return true;
918 }
919
920 /**
921 * @brief haveJointsTrajectories Check that a joint torque trajectories are
922 * defined for each phases Also check that the time interval of this
923 * trajectories matches the one of the phase and that the trajectories start
924 * and end and the correct values defined in each phase
925 * @return
926 */
927 ✗ bool haveTorquesTrajectories() const {
928 ✗ if (!haveTimings()) return false;
929 ✗ size_t i = 0;
930 ✗ for (const ContactPhase& phase : m_contact_phases) {
931 ✗ if (!phase.m_tau) {
932 ✗ std::cout << "Torque trajectory not defined for phase : " << i
933 ✗ << std::endl;
934 ✗ return false;
935 }
936
937 ✗ if (phase.m_tau->min() != phase.timeInitial()) {
938 ✗ std::cout << "Torque trajectory do not start at t_init for phase : "
939 ✗ << i << std::endl;
940 ✗ return false;
941 }
942 ✗ if ((phase.m_tau->max() != phase.timeFinal()) && i < size() - 1) {
943 ✗ std::cout << "Torque trajectory do not end at t_final for phase : " << i
944 ✗ << std::endl;
945 ✗ return false;
946 }
947 ✗ ++i;
948 }
949 ✗ return true;
950 }
951
952 /**
953 * @brief haveJointsTrajectories Check that a contact force trajectory exist
954 * for each active contact Also check that the time interval of this
955 * trajectories matches the one of the phase and that the trajectories start
956 * and end and the correct values defined in each phase
957 * @return
958 */
959 ✗ bool haveContactForcesTrajectories() const {
960 ✗ if (!haveTimings()) return false;
961 ✗ size_t i = 0;
962 ✗ for (const ContactPhase& phase : m_contact_phases) {
963 ✗ for (std::string eeName : phase.effectorsInContact()) {
964 ✗ if (phase.contactForces().count(eeName) == 0) {
965 std::cout << "No contact forces trajectory for effector " << eeName
966 ✗ << " at phase " << i << std::endl;
967 ✗ return false;
968 }
969 ✗ if (phase.contactNormalForces().count(eeName) == 0) {
970 std::cout << "No contact normal force trajectory for effector "
971 ✗ << eeName << " for phase " << i << std::endl;
972 ✗ return false;
973 }
974 ✗ if (phase.contactForces().at(eeName)->min() != phase.timeInitial()) {
975 std::cout << "Contact forces trajectory for effector " << eeName
976 ✗ << " do not start at t_init for phase " << i << std::endl;
977 ✗ return false;
978 }
979 ✗ if (phase.contactForces().at(eeName)->max() != phase.timeFinal()) {
980 std::cout << "Contact forces trajectory for effector " << eeName
981 ✗ << " do not end at t_final for phase " << i << std::endl;
982 ✗ return false;
983 }
984 ✗ if (phase.contactNormalForces().at(eeName)->min() !=
985 ✗ phase.timeInitial()) {
986 std::cout << "Contact normal force trajectory for effector " << eeName
987 ✗ << " do not start at t_init for phase " << i << std::endl;
988 ✗ return false;
989 }
990 ✗ if (phase.contactNormalForces().at(eeName)->max() !=
991 ✗ phase.timeFinal()) {
992 std::cout << "Contact normal force trajectory for effector " << eeName
993 ✗ << " do not end at t_final for phase " << i << std::endl;
994 ✗ return false;
995 }
996 }
997 ✗ ++i;
998 }
999 ✗ return true;
1000 }
1001
1002 /**
1003 * @brief haveRootTrajectories check that a root trajectory exist for each
1004 * contact phases. Also check that it start and end at the correct time
1005 * interval
1006 * @return
1007 */
1008 ✗ bool haveRootTrajectories() const {
1009 ✗ size_t i = 0;
1010 ✗ for (const ContactPhase& phase : m_contact_phases) {
1011 ✗ if (!phase.m_root) {
1012 ✗ std::cout << "Root trajectory not defined for phase : " << i
1013 ✗ << std::endl;
1014 ✗ return false;
1015 }
1016 ✗ if (phase.m_root->min() != phase.timeInitial()) {
1017 ✗ std::cout << "Root trajectory do not start at t_init for phase : " << i
1018 ✗ << std::endl;
1019 ✗ return false;
1020 }
1021 ✗ if (phase.m_root->max() != phase.timeFinal()) {
1022 ✗ std::cout << "Root trajectory do not start at t_final for phase : " << i
1023 ✗ << std::endl;
1024 ✗ return false;
1025 }
1026 ✗ ++i;
1027 }
1028 ✗ return true;
1029 }
1030
1031 /**
1032 * @brief haveFriction check that all the contact patch used in the sequence
1033 * have a friction coefficient initialized
1034 * @return
1035 */
1036 ✗ bool haveFriction() const {
1037 ✗ size_t i = 0;
1038 ✗ for (const ContactPhase& phase : m_contact_phases) {
1039 ✗ for (const std::string& eeName : phase.effectorsInContact()) {
1040 ✗ if (phase.contactPatches().at(eeName).friction() <= 0.) {
1041 ✗ std::cout << "Friction not defined for phase " << i
1042 ✗ << " and effector " << eeName << std::endl;
1043 ✗ return false;
1044 }
1045 }
1046 ✗ ++i;
1047 }
1048 ✗ return true;
1049 }
1050
1051 /**
1052 * @brief haveContactModelDefined check that all the contact patch have a
1053 * contact_model defined
1054 * @return
1055 */
1056 12 bool haveContactModelDefined() const {
1057 12 size_t i = 0;
1058
2/2
✓ Branch 4 taken 30 times.
✓ Branch 5 taken 4 times.
 34 for (const ContactPhase& phase : m_contact_phases) {
1059
5/6
✓ Branch 1 taken 30 times.
✗ Branch 2 not taken.
✓ Branch 8 taken 52 times.
✓ Branch 9 taken 22 times.
✓ Branch 11 taken 22 times.
✓ Branch 12 taken 8 times.
 82 for (const std::string& eeName : phase.effectorsInContact()) {
1060
4/6
✓ Branch 1 taken 52 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 52 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 8 times.
✓ Branch 8 taken 44 times.
 52 if (phase.contactPatches().at(eeName).m_contact_model.m_contact_type ==
1061 ContactType::CONTACT_UNDEFINED) {
1062
2/4
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
 8 std::cout << "ContactModel not defined for phase " << i
1063
3/6
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 8 times.
✗ Branch 8 not taken.
 8 << " and effector " << eeName << std::endl;
1064 8 return false;
1065 }
1066 }
1067 22 ++i;
1068 }
1069 4 return true;
1070 }
1071
1072 /**
1073 * @brief haveZMPtrajectories check that all the contact phases have a zmp
1074 * trajectory
1075 * @return
1076 */
1077 2 bool haveZMPtrajectories() {
1078 2 size_t i = 0;
1079
2/2
✓ Branch 4 taken 2 times.
✓ Branch 5 taken 1 times.
 3 for (const ContactPhase& phase : m_contact_phases) {
1080
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 2 times.
 2 if (!phase.m_zmp) {
1081 ✗ std::cout << "ZMP trajectory not defined for phase : " << i
1082 ✗ << std::endl;
1083 1 return false;
1084 }
1085
3/4
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 1 times.
✓ Branch 5 taken 1 times.
 2 if (phase.m_zmp->dim() != 3) {
1086
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 std::cout << "ZMP trajectory is not of dimension 3 for phase : " << i
1087
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 << std::endl;
1088 1 return false;
1089 }
1090
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✗ Branch 5 not taken.
✓ Branch 6 taken 1 times.
 1 if (phase.m_zmp->min() != phase.timeInitial()) {
1091 ✗ std::cout << "ZMP trajectory do not start at t_init for phase : " << i
1092 ✗ << std::endl;
1093 ✗ return false;
1094 }
1095
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✗ Branch 5 not taken.
✓ Branch 6 taken 1 times.
 1 if (phase.m_zmp->max() != phase.timeFinal()) {
1096 ✗ std::cout << "ZMP trajectory do not end at t_final for phase : " << i
1097 ✗ << std::endl;
1098 ✗ return false;
1099 }
1100 1 ++i;
1101 }
1102 1 return true;
1103 }
1104
1105 /**
1106 * @brief getAllEffectorsInContact return a vector of names of all the
1107 * effectors used to create contacts during the sequence
1108 * @return
1109 */
1110 ✗ t_strings getAllEffectorsInContact() const {
1111 // use set to guarantee uniqueness, but return a vector for easier use and
1112 // python bindings
1113 ✗ std::set<std::string> res_set;
1114 ✗ for (const ContactPhase& phase : m_contact_phases) {
1115 ✗ for (const std::string& eeName : phase.effectorsInContact()) {
1116 ✗ res_set.insert(eeName);
1117 }
1118 }
1119 ✗ return t_strings(res_set.begin(), res_set.end());
1120 }
1121
1122 /**
1123 * @brief concatenateCtrajectories Return a piecewise curve which is the
1124 * concatenation of the m_c curves for each contact phases in the sequence.
1125 * @return
1126 */
1127 1 piecewise_t concatenateCtrajectories() const {
1128 1 piecewise_t res = piecewise_t();
1129
2/2
✓ Branch 5 taken 2 times.
✓ Branch 6 taken 1 times.
 3 for (const ContactPhase& phase : m_contact_phases) {
1130
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 res.add_curve_ptr(phase.m_c);
1131 }
1132 1 return res;
1133 }
1134
1135 /**
1136 * @brief concatenateDCtrajectories Return a piecewise curve which is the
1137 * concatenation of the m_dc curves for each contact phases in the sequence.
1138 * @return
1139 */
1140 ✗ piecewise_t concatenateDCtrajectories() const {
1141 ✗ piecewise_t res = piecewise_t();
1142 ✗ for (const ContactPhase& phase : m_contact_phases) {
1143 ✗ res.add_curve_ptr(phase.m_dc);
1144 }
1145 ✗ return res;
1146 }
1147
1148 /**
1149 * @brief concatenateDDCtrajectories Return a piecewise curve which is the
1150 * concatenation of the m_ddc curves for each contact phases in the sequence.
1151 * @return
1152 */
1153 ✗ piecewise_t concatenateDDCtrajectories() const {
1154 ✗ piecewise_t res = piecewise_t();
1155 ✗ for (const ContactPhase& phase : m_contact_phases) {
1156 ✗ res.add_curve_ptr(phase.m_ddc);
1157 }
1158 ✗ return res;
1159 }
1160
1161 /**
1162 * @brief concatenateLtrajectories Return a piecewise curve which is the
1163 * concatenation of the m_L curves for each contact phases in the sequence.
1164 * @return
1165 */
1166 ✗ piecewise_t concatenateLtrajectories() const {
1167 ✗ piecewise_t res = piecewise_t();
1168 ✗ for (const ContactPhase& phase : m_contact_phases) {
1169 ✗ res.add_curve_ptr(phase.m_L);
1170 }
1171 ✗ return res;
1172 }
1173
1174 /**
1175 * @brief concatenateDLtrajectories Return a piecewise curve which is the
1176 * concatenation of the m_dL curves for each contact phases in the sequence.
1177 * @return
1178 */
1179 ✗ piecewise_t concatenateDLtrajectories() const {
1180 ✗ piecewise_t res = piecewise_t();
1181 ✗ for (const ContactPhase& phase : m_contact_phases) {
1182 ✗ res.add_curve_ptr(phase.m_dL);
1183 }
1184 ✗ return res;
1185 }
1186
1187 /**
1188 * @brief concatenateZMPtrajectories Return a piecewise curve which is the
1189 * concatenation of the m_zmp curves for each contact phases in the sequence.
1190 * @return
1191 */
1192 ✗ piecewise_t concatenateZMPtrajectories() const {
1193 ✗ piecewise_t res = piecewise_t();
1194 ✗ for (const ContactPhase& phase : m_contact_phases) {
1195 ✗ res.add_curve_ptr(phase.m_zmp);
1196 }
1197 ✗ return res;
1198 }
1199
1200 /**
1201 * @brief concatenateWrenchTrajectories Return a piecewise curve which is the
1202 * concatenation of the m_wrench curves for each contact phases in the
1203 * sequence.
1204 * @return
1205 */
1206 ✗ piecewise_t concatenateWrenchTrajectories() const {
1207 ✗ piecewise_t res = piecewise_t();
1208 ✗ for (const ContactPhase& phase : m_contact_phases) {
1209 ✗ res.add_curve_ptr(phase.m_wrench);
1210 }
1211 ✗ return res;
1212 }
1213
1214 /**
1215 * @brief concatenateQtrajectories Return a piecewise curve which is the
1216 * concatenation of the m_q curves for each contact phases in the sequence.
1217 * @return
1218 */
1219 1 piecewise_t concatenateQtrajectories() const {
1220 1 piecewise_t res = piecewise_t();
1221
2/2
✓ Branch 5 taken 2 times.
✓ Branch 6 taken 1 times.
 3 for (const ContactPhase& phase : m_contact_phases) {
1222
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 res.add_curve_ptr(phase.m_q);
1223 }
1224 1 return res;
1225 }
1226
1227 /**
1228 * @brief concatenateDQtrajectories Return a piecewise curve which is the
1229 * concatenation of the m_dq curves for each contact phases in the sequence.
1230 * @return
1231 */
1232 ✗ piecewise_t concatenateDQtrajectories() const {
1233 ✗ piecewise_t res = piecewise_t();
1234 ✗ for (const ContactPhase& phase : m_contact_phases) {
1235 ✗ res.add_curve_ptr(phase.m_dq);
1236 }
1237 ✗ return res;
1238 }
1239
1240 /**
1241 * @brief concatenateDDQtrajectories Return a piecewise curve which is the
1242 * concatenation of the m_ddq curves for each contact phases in the sequence.
1243 * @return
1244 */
1245 ✗ piecewise_t concatenateDDQtrajectories() const {
1246 ✗ piecewise_t res = piecewise_t();
1247 ✗ for (const ContactPhase& phase : m_contact_phases) {
1248 ✗ res.add_curve_ptr(phase.m_ddq);
1249 }
1250 ✗ return res;
1251 }
1252
1253 /**
1254 * @brief concatenateTauTrajectories Return a piecewise curve which is the
1255 * concatenation of the m_tau curves for each contact phases in the sequence.
1256 * @return
1257 */
1258 ✗ piecewise_t concatenateTauTrajectories() const {
1259 ✗ piecewise_t res = piecewise_t();
1260 ✗ for (const ContactPhase& phase : m_contact_phases) {
1261 ✗ res.add_curve_ptr(phase.m_tau);
1262 }
1263 ✗ return res;
1264 }
1265
1266 /**
1267 * @brief concatenateRootTrajectories Return a piecewise curve which is the
1268 * concatenation of the m_root curves for each contact phases in the sequence.
1269 * @return
1270 */
1271 ✗ piecewise_SE3_t concatenateRootTrajectories() const {
1272 ✗ piecewise_SE3_t res = piecewise_SE3_t();
1273 ✗ for (const ContactPhase& phase : m_contact_phases) {
1274 ✗ res.add_curve_ptr(phase.m_root);
1275 }
1276 ✗ return res;
1277 }
1278
1279 /**
1280 * @brief concatenateEffectorTrajectories Return a piecewise curve which is
1281 * the concatenation of the effectors trajectories curves for the given
1282 * effector for each contact phases in the sequence. During the phases where
1283 * no effector trajectories are defined, the trajectory is constant with the
1284 * value of the last phase where it was defined.
1285 * @param eeName the effector name
1286 * @return
1287 */
1288 6 piecewise_SE3_t concatenateEffectorTrajectories(
1289 const std::string& eeName) const {
1290
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 6 piecewise_SE3_t res = piecewise_SE3_t();
1291
2/4
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
 6 transform_t last_placement, first_placement;
1292 // first find the first and last phase with a trajectory for this effector
1293 6 size_t first_phase = m_contact_phases.size();
1294 6 size_t last_phase = 0;
1295
2/2
✓ Branch 1 taken 22 times.
✓ Branch 2 taken 6 times.
 28 for (size_t i = 0; i < m_contact_phases.size(); ++i) {
1296
4/6
✓ Branch 1 taken 22 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 22 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 10 times.
✓ Branch 7 taken 12 times.
 22 if (m_contact_phases.at(i).effectorHaveAtrajectory(eeName)) {
1297 10 last_phase = i;
1298
2/2
✓ Branch 0 taken 5 times.
✓ Branch 1 taken 5 times.
 10 if (first_phase > i) {
1299 5 first_phase = i;
1300 5 curve_SE3_ptr curve =
1301
3/6
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 5 times.
✗ Branch 8 not taken.
 5 m_contact_phases.at(i).effectorTrajectories().at(eeName);
1302
2/4
✓ Branch 3 taken 5 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 5 times.
✗ Branch 7 not taken.
 5 first_placement = curve->operator()(curve->min());
1303 5 }
1304 }
1305 }
1306
2/2
✓ Branch 1 taken 1 times.
✓ Branch 2 taken 5 times.
 6 if (first_phase == m_contact_phases.size())
1307
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 throw std::invalid_argument(
1308 "The contact sequence doesn't have any phase with an effector "
1309 "trajectory"
1310 " for the given effector name");
1311
2/2
✓ Branch 0 taken 2 times.
✓ Branch 1 taken 3 times.
 5 if (first_phase > 0) {
1312 // add a first constant phase at the initial placement
1313
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 curve_SE3_ptr ptr_init(
1314
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 new SE3Curve_t(first_placement, first_placement,
1315
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 m_contact_phases.at(0).timeInitial(),
1316
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
 2 m_contact_phases.at(first_phase).timeInitial()));
1317
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 res.add_curve_ptr(ptr_init);
1318 2 }
1319 // loop over this phases to concatenate the trajectories
1320
2/2
✓ Branch 0 taken 15 times.
✓ Branch 1 taken 5 times.
 20 for (size_t i = first_phase; i <= last_phase; ++i) {
1321
4/6
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 15 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 10 times.
✓ Branch 7 taken 5 times.
 15 if (m_contact_phases.at(i).effectorHaveAtrajectory(eeName)) {
1322 10 res.add_curve_ptr(
1323
4/8
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 10 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 10 times.
✗ Branch 11 not taken.
 10 m_contact_phases.at(i).effectorTrajectories().at(eeName));
1324
1/2
✓ Branch 2 taken 10 times.
✗ Branch 3 not taken.
 10 last_placement = res(res.max());
1325 } else {
1326 // when the trajectory do not exist, add a constant one with the
1327 // previous value
1328
2/4
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
 5 curve_SE3_ptr ptr(new SE3Curve_t(last_placement, last_placement,
1329
1/2
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
 5 m_contact_phases.at(i).timeInitial(),
1330
2/4
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 5 times.
✗ Branch 6 not taken.
 5 m_contact_phases.at(i).timeFinal()));
1331
1/2
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
 5 res.add_curve_ptr(ptr);
1332 5 }
1333 }
1334
2/2
✓ Branch 1 taken 2 times.
✓ Branch 2 taken 3 times.
 5 if (last_phase < m_contact_phases.size() - 1) {
1335 // add a last constant phase until the end of the contact sequence
1336
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 curve_SE3_ptr ptr_final(
1337
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 new SE3Curve_t(last_placement, last_placement,
1338
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 m_contact_phases.at(last_phase).timeFinal(),
1339
1/2
✓ Branch 3 taken 2 times.
✗ Branch 4 not taken.
 2 m_contact_phases.back().timeFinal()));
1340
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 res.add_curve_ptr(ptr_final);
1341 2 }
1342 10 return res;
1343 1 }
1344
1345 /**
1346 * @brief concatenateContactForceTrajectories Return a piecewise curve which
1347 * is the concatenation of the contact forces for the given effector
1348 * for each contact phases in the sequence.
1349 * During the phases where no contact forces are defined, the trajectory is
1350 * constant with the value of 0.
1351 * @param eeName the effector name
1352 * @return
1353 */
1354 2 piecewise_t concatenateContactForceTrajectories(
1355 const std::string& eeName) const {
1356
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 piecewise_t res = piecewise_t();
1357 // first find the dimension used, in order to fill with 0 when required:
1358 2 size_t dim = 0;
1359
2/2
✓ Branch 5 taken 6 times.
✓ Branch 6 taken 2 times.
 8 for (const ContactPhase& phase : m_contact_phases) {
1360
4/6
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4 times.
✓ Branch 8 taken 2 times.
 6 if (phase.contactForces().count(eeName) > 0) {
1361
3/6
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 4 times.
✗ Branch 9 not taken.
 4 dim = phase.contactForces().at(eeName)->dim();
1362 }
1363 }
1364
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 2 times.
 2 if (dim == 0) {
1365 // no forces trajectory for this effector
1366 ✗ return res;
1367 }
1368
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 2 Eigen::MatrixXd zeros = Eigen::MatrixXd::Zero(dim, 1);
1369 // loop over all phases and either add trajectory if it exist or add zeros
1370
2/2
✓ Branch 5 taken 6 times.
✓ Branch 6 taken 2 times.
 8 for (const ContactPhase& phase : m_contact_phases) {
1371
4/6
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4 times.
✓ Branch 8 taken 2 times.
 6 if (phase.contactForces().count(eeName) > 0) {
1372
3/6
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4 times.
✗ Branch 8 not taken.
 4 res.add_curve_ptr(phase.contactForces().at(eeName));
1373 } else {
1374
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 curve_ptr ptr(
1375
2/4
✓ Branch 3 taken 2 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 2 times.
✗ Branch 7 not taken.
 2 new polynomial_t(zeros, phase.timeInitial(), phase.timeFinal()));
1376
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 res.add_curve_ptr(ptr);
1377 2 }
1378 }
1379 2 return res;
1380 2 }
1381
1382 /**
1383 * @brief concatenateNormalForceTrajectories Return a piecewise curve which
1384 * is the concatenation of the contact normal forces for the given effector
1385 * for each contact phases in the sequence.
1386 * During the phases where no contact normal forces are defined, the
1387 * trajectory is constant with the value of 0.
1388 * @param eeName the effector name
1389 * @return
1390 */
1391 2 piecewise_t concatenateNormalForceTrajectories(
1392 const std::string& eeName) const {
1393
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 piecewise_t res = piecewise_t();
1394
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 2 Eigen::MatrixXd zeros = Eigen::MatrixXd::Zero(1, 1);
1395 // loop over all phases and either add trajectory if it exist or add zeros
1396
2/2
✓ Branch 5 taken 6 times.
✓ Branch 6 taken 2 times.
 8 for (const ContactPhase& phase : m_contact_phases) {
1397
4/6
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2 times.
✓ Branch 8 taken 4 times.
 6 if (phase.contactNormalForces().count(eeName) > 0) {
1398
3/6
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
 2 res.add_curve_ptr(phase.contactNormalForces().at(eeName));
1399 } else {
1400
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 4 curve_ptr ptr(
1401
2/4
✓ Branch 3 taken 4 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 4 times.
✗ Branch 7 not taken.
 4 new polynomial_t(zeros, phase.timeInitial(), phase.timeFinal()));
1402
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 4 res.add_curve_ptr(ptr);
1403 4 }
1404 }
1405 4 return res;
1406 2 }
1407
1408 /**
1409 * @brief phaseAtTime return a phase of the sequence such that :
1410 * phase.timeInitial <= t < phase.timeFinal
1411 * if t equal to the last phase timeFinal, this index is returned
1412 * @param time
1413 * @return the phase whose time interval contains 'time'
1414 * @throw invalid_argument error if no phase are found for this time
1415 */
1416 20 ContactPhase& phaseAtTime(const double time) {
1417 20 const int id = phaseIdAtTime(time);
1418
2/2
✓ Branch 0 taken 16 times.
✓ Branch 1 taken 4 times.
 20 if (id >= 0)
1419 16 return m_contact_phases.at(id);
1420 else
1421
1/2
✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
 4 throw std::invalid_argument("No phase found for the given time.");
1422 }
1423
1424 /**
1425 * @brief phaseIdAtTime return the index of a phase in the sequence such that
1426 * : phase.timeInitial <= t < phase.timeFinal if t equal to the last phase
1427 * timeFinal, this index is returned
1428 * @param time
1429 * @return the index of the phase whose time interval contain 'time', -1 if no
1430 * phase are found
1431 */
1432 40 int phaseIdAtTime(const double time) const {
1433
2/2
✓ Branch 1 taken 88 times.
✓ Branch 2 taken 12 times.
 100 for (int i = 0; i < static_cast<int>(m_contact_phases.size()); ++i) {
1434 88 const ContactPhase& phase = m_contact_phases[i];
1435
6/6
✓ Branch 1 taken 76 times.
✓ Branch 2 taken 12 times.
✓ Branch 4 taken 28 times.
✓ Branch 5 taken 48 times.
✓ Branch 6 taken 28 times.
✓ Branch 7 taken 60 times.
 88 if (time >= phase.timeInitial() && time < phase.timeFinal()) {
1436 28 return i;
1437 }
1438 }
1439
2/2
✓ Branch 2 taken 4 times.
✓ Branch 3 taken 8 times.
 12 if (time == m_contact_phases.back().timeFinal())
1440 4 return static_cast<int>(m_contact_phases.size() - 1);
1441 8 return -1;
1442 }
1443
1444 /* End Helpers */
1445
1446 /*Public Attributes*/
1447 ContactPhaseVector m_contact_phases;
1448 /*Public Attributes*/
1449
1450 private:
1451 // Serialization of the class
1452 friend class boost::serialization::access;
1453
1454 template <class Archive>
1455 14 void save(Archive& ar, const unsigned int /*version*/) const {
1456 14 const size_t m_size = size();
1457
1/2
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
 14 ar& boost::serialization::make_nvp("size", m_size);
1458 14 for (typename ContactPhaseVector::const_iterator it =
1459 14 m_contact_phases.begin();
1460
2/2
✓ Branch 2 taken 70 times.
✓ Branch 3 taken 7 times.
 154 it != m_contact_phases.end(); ++it) {
1461
1/2
✓ Branch 3 taken 70 times.
✗ Branch 4 not taken.
 140 ar& boost::serialization::make_nvp("contact_phase", *it);
1462 }
1463 14 }
1464
1465 template <class Archive>
1466 14 void load(Archive& ar, const unsigned int /*version*/) {
1467 size_t m_size;
1468
1/2
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
 14 ar >> boost::serialization::make_nvp("size", m_size);
1469
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 7 times.
 14 assert(m_size > 0);
1470
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 14 resize(m_size);
1471 14 for (typename ContactPhaseVector::iterator it = m_contact_phases.begin();
1472
2/2
✓ Branch 2 taken 70 times.
✓ Branch 3 taken 7 times.
 154 it != m_contact_phases.end(); ++it) {
1473
1/2
✓ Branch 3 taken 70 times.
✗ Branch 4 not taken.
 140 ar >> boost::serialization::make_nvp("contact_phase", *it);
1474 }
1475 14 }
1476
1477 28 BOOST_SERIALIZATION_SPLIT_MEMBER()
1478
1479 }; // end class ContactSequence
1480
1481 } // namespace scenario
1482 } // namespace multicontact_api
1483
1484 MULTICONTACT_API_DEFINE_CLASS_TEMPLATE_VERSION(
1485 typename Scalar, multicontact_api::scenario::ContactSequenceTpl<Scalar>)
1486
1487 #endif // __multicontact_api_scenario_contact_sequence_hpp__
1488