Loading...
Searching...
No Matches
contact-sequence.hpp
Go to the documentation of this file.
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
16
17namespace multicontact_api {
18namespace scenario {
19
20template <class _ContactPhase>
22 : public serialization::Serializable<ContactSequenceTpl<_ContactPhase> > {
23 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
24 typedef _ContactPhase ContactPhase;
25 typedef typename ContactPhase::Scalar Scalar;
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 boost::shared_ptr<curve_t> curve_ptr;
35 typedef boost::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
42
46
47 size_t size() const { return m_contact_phases.size(); }
48
49 bool operator==(const ContactSequenceTpl& other) const {
50 return m_contact_phases == other.m_contact_phases;
51 }
52
53 bool operator!=(const ContactSequenceTpl& other) const {
54 return !(*this == other);
55 }
56
57 void resize(const size_t size) { m_contact_phases.resize(size); }
58
59 /* Accessors to the contact Phases */
60
68 return m_contact_phases.size() - 1;
69 }
70
78
85 ContactPhase& contactPhase(const size_t id) {
86 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 return m_contact_phases.at(id);
91 }
92
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 */
121 void breakContact(const std::string& eeName,
122 const double phaseDuration = -1) {
123 ContactPhase& lastPhase = m_contact_phases.back();
124 if (!lastPhase.isEffectorInContact(eeName))
125 throw std::invalid_argument(
126 "In breakContact : effector is not currently in contact : " + eeName);
127 if (phaseDuration > 0) {
128 if (lastPhase.timeInitial() < 0) {
129 throw std::invalid_argument(
130 "In breakContact : duration is specified but current phase "
131 "interval in not initialised.");
132 } else {
133 lastPhase.duration(phaseDuration);
134 }
135 }
136 ContactPhase phase;
137 // set initial values from last values of previous phase :
138 phase.timeInitial(lastPhase.timeFinal());
139 phase.m_c_init = lastPhase.m_c_final;
140 phase.m_dc_init = lastPhase.m_dc_final;
141 phase.m_ddc_init = lastPhase.m_ddc_final;
142 phase.m_L_init = lastPhase.m_L_final;
143 phase.m_dL_init = lastPhase.m_dL_final;
144 phase.m_q_init = lastPhase.m_q_final;
145
146 // copy contact patchs :
147 for (std::string name : lastPhase.effectorsInContact()) {
148 if (name != eeName) {
149 phase.addContact(name, lastPhase.contactPatch(name));
150 }
151 }
152 // add new phase at the end of the sequence
153 append(phase);
154 return;
155 }
156
171 void createContact(const std::string& eeName, const ContactPatch& patch,
172 const double phaseDuration = -1) {
173 ContactPhase& lastPhase = m_contact_phases.back();
174 if (lastPhase.isEffectorInContact(eeName))
175 throw std::invalid_argument(
176 "In createContact : effector is already in contact : " + eeName);
177 if (phaseDuration > 0) {
178 if (lastPhase.timeInitial() < 0) {
179 throw std::invalid_argument(
180 "In createContact : duration is specified but current phase "
181 "interval in not initialised.");
182 } else {
183 lastPhase.duration(phaseDuration);
184 }
185 }
186 ContactPhase phase;
187 // set initial values from last values of previous phase :
188 phase.timeInitial(lastPhase.timeFinal());
189 phase.m_c_init = lastPhase.m_c_final;
190 phase.m_dc_init = lastPhase.m_dc_final;
191 phase.m_ddc_init = lastPhase.m_ddc_final;
192 phase.m_L_init = lastPhase.m_L_final;
193 phase.m_dL_init = lastPhase.m_dL_final;
194 phase.m_q_init = lastPhase.m_q_final;
195
196 // copy contact patchs :
197 for (std::string name : lastPhase.effectorsInContact()) {
198 phase.addContact(name, lastPhase.contactPatch(name));
199 }
200 // add new contact to new phase :
201 phase.addContact(eeName, patch);
202 // add new phase at the end of the sequence
203 append(phase);
204 return;
205 }
206
225 void moveEffectorToPlacement(const std::string& eeName,
226 const ContactPatch::SE3& placement,
227 const double durationBreak = -1,
228 const double durationCreate = -1) {
229 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 ContactPatch target(m_contact_phases.back().contactPatch(eeName));
235 target.placement() = placement;
236 breakContact(eeName, durationBreak);
237 // copy all "init" value to "final" for the current last phase :
238 ContactPhase& lastPhase = m_contact_phases.back();
239 lastPhase.m_c_final = lastPhase.m_c_init;
240 lastPhase.m_dc_final = lastPhase.m_dc_init;
241 lastPhase.m_ddc_final = lastPhase.m_ddc_init;
242 lastPhase.m_L_final = lastPhase.m_L_init;
243 lastPhase.m_dL_final = lastPhase.m_dL_init;
244 lastPhase.m_q_final = lastPhase.m_q_init;
245 createContact(eeName, target, durationCreate);
246 return;
247 }
248
265 void moveEffectorOf(const std::string& eeName,
266 const ContactPatch::SE3& transform,
267 const double durationBreak = -1,
268 const double durationCreate = -1) {
269 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 ContactPatch::SE3 previous(
275 m_contact_phases.back().contactPatch(eeName).placement());
276 ContactPatch::SE3 target = transform.act(previous);
277 return moveEffectorToPlacement(eeName, target, durationBreak,
278 durationCreate);
279 }
280
287 bool haveTimings() const {
288 double current_t = m_contact_phases.front().timeInitial();
289 if (current_t < 0.) {
290 std::cout << "Initial time is negative." << std::endl;
291 return false;
292 }
293 for (size_t i = 0; i < size(); ++i) {
294 const ContactPhase& phase = m_contact_phases.at(i);
295 if (phase.timeInitial() != current_t) {
296 std::cout << "For phase " << i
297 << " initial time is not equal to previous final time."
298 << std::endl;
299 return false;
300 }
301 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 current_t = phase.timeFinal();
307 }
308 return true;
309 }
310
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
364 bool haveCOMvalues() const {
365 if (m_contact_phases.front().m_c_init.isZero()) {
366 std::cout << "Initial CoM position not defined." << std::endl;
367 return false;
368 }
369 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 if (m_contact_phases.back().m_c_final.isZero()) {
402 std::cout << "Final CoM position not defined." << std::endl;
403 return false;
404 }
405 return true;
406 }
407
414 bool haveAMvalues() const {
415 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 return true;
432 }
433
440 bool haveCentroidalValues() const {
441 return haveAMvalues() && haveCOMvalues();
442 }
443
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
483 bool haveCOMtrajectories() const {
484 if (!(haveTimings() && haveCOMvalues())) return false;
485 size_t i = 0;
486 for (const ContactPhase& phase : m_contact_phases) {
487 if (!phase.m_c) {
488 std::cout << "CoM position trajectory not defined for phase : " << i
489 << std::endl;
490 return false;
491 }
492 if (!phase.m_dc) {
493 std::cout << "CoM velocity trajectory not defined for phase : " << i
494 << std::endl;
495 return false;
496 }
497 if (!phase.m_ddc) {
498 std::cout << "CoM acceleration trajectory not defined for phase : " << i
499 << std::endl;
500 return false;
501 }
502 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 if (phase.m_dc->dim() != 3) {
508 std::cout
509 << "CoM velocity trajectory is not of dimension 3 for phase : " << i
510 << std::endl;
511 return false;
512 }
513 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 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 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 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 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 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 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 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 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 else if (!(*phase.m_dc)(phase.m_dc->min())
568 .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 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 } else if (!(*phase.m_ddc)(phase.m_ddc->min())
582 .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 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 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 } else if (!(*phase.m_dc)(phase.m_dc->max())
601 .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 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 } else if (!(*phase.m_ddc)(phase.m_ddc->max())
615 .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 ++i;
622 }
623 return true;
624 }
625
633 bool haveAMtrajectories() const {
634 if (!(haveTimings() && haveAMvalues())) return false;
635 size_t i = 0;
636 for (const ContactPhase& phase : m_contact_phases) {
637 if (!phase.m_L) {
638 std::cout << "AM position trajectory not defined for phase : " << i
639 << std::endl;
640 return false;
641 }
642 if (!phase.m_dL) {
643 std::cout << "AM velocity trajectory not defined for phase : " << i
644 << std::endl;
645 return false;
646 }
647 if (phase.m_L->dim() != 3) {
648 std::cout << "AM trajectory is not of dimension 3 for phase : " << i
649 << std::endl;
650 return false;
651 }
652 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 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 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 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 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 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 } 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 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 } else if (!(*phase.m_dL)(phase.m_dL->min())
699 .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 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 } else if (!(*phase.m_L)(phase.m_L->max())
712 .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 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 } 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 ++i;
731 }
732 return true;
733 }
734
744 }
745
757 const Scalar prec = Eigen::NumTraits<Scalar>::dummy_precision(),
758 const bool use_rotation = true) const {
759 if (!haveTimings()) return false;
760 for (size_t i = 0; i < m_contact_phases.size() - 1; ++i) {
761 for (std::string eeName : m_contact_phases.at(i).getContactsCreated(
762 m_contact_phases.at(i + 1))) {
763 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 const typename ContactPhase::curve_SE3_ptr traj =
770 m_contact_phases.at(i).effectorTrajectories().at(eeName);
771 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 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 ContactPatch::SE3 pMax =
782 ContactPatch::SE3((*traj)(traj->max()).matrix());
783 if ((use_rotation && !pMax.isApprox(m_contact_phases.at(i + 1)
784 .contactPatches()
785 .at(eeName)
786 .placement(),
787 prec)) ||
788 (!pMax.translation().isApprox(m_contact_phases.at(i + 1)
789 .contactPatches()
790 .at(eeName)
791 .placement()
792 .translation(),
793 prec))) {
794 std::cout << "Effector trajectory for " << eeName
795 << " do not end at it's contact placement in the next "
796 "phase, for phase "
797 << i << std::endl;
798 std::cout << "Last point : " << std::endl
799 << pMax << std::endl
800 << "Next contact : " << std::endl
801 << m_contact_phases.at(i + 1)
802 .contactPatches()
803 .at(eeName)
804 .placement()
805 << std::endl;
806 return false;
807 }
808 if (i > 0 && m_contact_phases.at(i - 1).isEffectorInContact(eeName)) {
809 ContactPatch::SE3 pMin =
810 ContactPatch::SE3((*traj)(traj->min()).matrix());
811 if ((use_rotation && !pMin.isApprox(m_contact_phases.at(i - 1)
812 .contactPatches()
813 .at(eeName)
814 .placement(),
815 prec)) ||
816 (!pMin.translation().isApprox(m_contact_phases.at(i - 1)
817 .contactPatches()
818 .at(eeName)
819 .placement()
820 .translation(),
821 prec))) {
822 std::cout << "Effector trajectory for " << eeName
823 << " do not start at it's contact placement in the "
824 "previous phase, for phase "
825 << i << std::endl;
826 std::cout << "First point : " << std::endl
827 << pMin << std::endl
828 << "Previous contact : " << std::endl
829 << m_contact_phases.at(i - 1)
830 .contactPatches()
831 .at(eeName)
832 .placement()
833 << std::endl;
834 return false;
835 }
836 }
837 }
838 }
839 return true;
840 }
841
850 if (!(haveTimings() && haveConfigurationsValues())) return false;
851 size_t i = 0;
852 for (const ContactPhase& phase : m_contact_phases) {
853 if (!phase.m_q) {
854 std::cout << "joint position trajectory not defined for phase : " << i
855 << std::endl;
856 return false;
857 }
858 if (phase.m_q->min() != phase.timeInitial()) {
859 std::cout << "joint trajectory do not start at t_init for phase : " << i
860 << std::endl;
861 return false;
862 }
863 if (phase.m_q->max() != phase.timeFinal()) {
864 std::cout << "joint trajectory do not end at t_final for phase : " << i
865 << std::endl;
866 return false;
867 }
868 if (!(*phase.m_q)(phase.m_q->min()).isApprox(phase.m_q_init)) {
869 std::cout << "joint trajectory do not start at q_init for phase : " << i
870 << std::endl;
871 return false;
872 }
873 if (!(*phase.m_q)(phase.m_q->max()).isApprox(phase.m_q_final)) {
874 std::cout << "joint trajectory do not end at q_final for phase : " << i
875 << std::endl;
876 return false;
877 }
878 ++i;
879 }
880 return true;
881 }
882
891 if (!(haveTimings())) return false;
892 size_t i = 0;
893 for (const ContactPhase& phase : m_contact_phases) {
894 if (!phase.m_dq) {
895 std::cout << "joint velocity trajectory not defined for phase : " << i
896 << std::endl;
897 return false;
898 }
899 if (!phase.m_ddq) {
900 std::cout << "joint acceleration trajectory not defined for phase : "
901 << i << std::endl;
902 return false;
903 }
904 if (phase.m_dq->min() != phase.timeInitial()) {
905 std::cout
906 << "joint velocity trajectory do not start at t_init for phase : "
907 << i << std::endl;
908 return false;
909 }
910 if (phase.m_ddq->min() != phase.timeInitial()) {
911 std::cout << "joint acceleration trajectory do not start at t_init for "
912 "phase : "
913 << i << std::endl;
914 return false;
915 }
916 if (phase.m_dq->max() != phase.timeFinal()) {
917 std::cout
918 << "joint velocity trajectory do not end at t_final for phase : "
919 << i << std::endl;
920 return false;
921 }
922 if ((phase.m_ddq->max() != phase.timeFinal()) &&
923 i < size() - 1) { // not required for the last phase
924 std::cout << "joint acceleration trajectory do not end at t_final for "
925 "phase : "
926 << i << std::endl;
927 return false;
928 }
929 ++i;
930 }
931 return true;
932 }
933
942 if (!haveTimings()) return false;
943 size_t i = 0;
944 for (const ContactPhase& phase : m_contact_phases) {
945 if (!phase.m_tau) {
946 std::cout << "Torque trajectory not defined for phase : " << i
947 << std::endl;
948 return false;
949 }
950
951 if (phase.m_tau->min() != phase.timeInitial()) {
952 std::cout << "Torque trajectory do not start at t_init for phase : "
953 << i << std::endl;
954 return false;
955 }
956 if ((phase.m_tau->max() != phase.timeFinal()) && i < size() - 1) {
957 std::cout << "Torque trajectory do not end at t_final for phase : " << i
958 << std::endl;
959 return false;
960 }
961 ++i;
962 }
963 return true;
964 }
965
974 if (!haveTimings()) return false;
975 size_t i = 0;
976 for (const ContactPhase& phase : m_contact_phases) {
977 for (std::string eeName : phase.effectorsInContact()) {
978 if (phase.contactForces().count(eeName) == 0) {
979 std::cout << "No contact forces trajectory for effector " << eeName
980 << " at phase " << i << std::endl;
981 return false;
982 }
983 if (phase.contactNormalForces().count(eeName) == 0) {
984 std::cout << "No contact normal force trajectory for effector "
985 << eeName << " for phase " << i << std::endl;
986 return false;
987 }
988 if (phase.contactForces().at(eeName)->min() != phase.timeInitial()) {
989 std::cout << "Contact forces trajectory for effector " << eeName
990 << " do not start at t_init for phase " << i << std::endl;
991 return false;
992 }
993 if (phase.contactForces().at(eeName)->max() != phase.timeFinal()) {
994 std::cout << "Contact forces trajectory for effector " << eeName
995 << " do not end at t_final for phase " << i << std::endl;
996 return false;
997 }
998 if (phase.contactNormalForces().at(eeName)->min() !=
999 phase.timeInitial()) {
1000 std::cout << "Contact normal force trajectory for effector " << eeName
1001 << " do not start at t_init for phase " << i << std::endl;
1002 return false;
1003 }
1004 if (phase.contactNormalForces().at(eeName)->max() !=
1005 phase.timeFinal()) {
1006 std::cout << "Contact normal force trajectory for effector " << eeName
1007 << " do not end at t_final for phase " << i << std::endl;
1008 return false;
1009 }
1010 }
1011 ++i;
1012 }
1013 return true;
1014 }
1015
1023 size_t i = 0;
1024 for (const ContactPhase& phase : m_contact_phases) {
1025 if (!phase.m_root) {
1026 std::cout << "Root trajectory not defined for phase : " << i
1027 << std::endl;
1028 return false;
1029 }
1030 if (phase.m_root->min() != phase.timeInitial()) {
1031 std::cout << "Root trajectory do not start at t_init for phase : " << i
1032 << std::endl;
1033 return false;
1034 }
1035 if (phase.m_root->max() != phase.timeFinal()) {
1036 std::cout << "Root trajectory do not start at t_final for phase : " << i
1037 << std::endl;
1038 return false;
1039 }
1040 ++i;
1041 }
1042 return true;
1043 }
1044
1050 bool haveFriction() const {
1051 size_t i = 0;
1052 for (const ContactPhase& phase : m_contact_phases) {
1053 for (const std::string& eeName : phase.effectorsInContact()) {
1054 if (phase.contactPatches().at(eeName).friction() <= 0.) {
1055 std::cout << "Friction not defined for phase " << i
1056 << " and effector " << eeName << std::endl;
1057 return false;
1058 }
1059 }
1060 ++i;
1061 }
1062 return true;
1063 }
1064
1071 size_t i = 0;
1072 for (const ContactPhase& phase : m_contact_phases) {
1073 for (const std::string& eeName : phase.effectorsInContact()) {
1074 if (phase.contactPatches().at(eeName).m_contact_model.m_contact_type ==
1076 std::cout << "ContactModel not defined for phase " << i
1077 << " and effector " << eeName << std::endl;
1078 return false;
1079 }
1080 }
1081 ++i;
1082 }
1083 return true;
1084 }
1085
1092 size_t i = 0;
1093 for (const ContactPhase& phase : m_contact_phases) {
1094 if (!phase.m_zmp) {
1095 std::cout << "ZMP trajectory not defined for phase : " << i
1096 << std::endl;
1097 return false;
1098 }
1099 if (phase.m_zmp->dim() != 3) {
1100 std::cout << "ZMP trajectory is not of dimension 3 for phase : " << i
1101 << std::endl;
1102 return false;
1103 }
1104 if (phase.m_zmp->min() != phase.timeInitial()) {
1105 std::cout << "ZMP trajectory do not start at t_init for phase : " << i
1106 << std::endl;
1107 return false;
1108 }
1109 if (phase.m_zmp->max() != phase.timeFinal()) {
1110 std::cout << "ZMP trajectory do not end at t_final for phase : " << i
1111 << std::endl;
1112 return false;
1113 }
1114 ++i;
1115 }
1116 return true;
1117 }
1118
1125 // use set to guarantee uniqueness, but return a vector for easier use and
1126 // python bindings
1127 std::set<std::string> res_set;
1128 for (const ContactPhase& phase : m_contact_phases) {
1129 for (const std::string& eeName : phase.effectorsInContact()) {
1130 res_set.insert(eeName);
1131 }
1132 }
1133 return t_strings(res_set.begin(), res_set.end());
1134 }
1135
1142 piecewise_t res = piecewise_t();
1143 for (const ContactPhase& phase : m_contact_phases) {
1144 res.add_curve_ptr(phase.m_c);
1145 }
1146 return res;
1147 }
1148
1155 piecewise_t res = piecewise_t();
1156 for (const ContactPhase& phase : m_contact_phases) {
1157 res.add_curve_ptr(phase.m_dc);
1158 }
1159 return res;
1160 }
1161
1168 piecewise_t res = piecewise_t();
1169 for (const ContactPhase& phase : m_contact_phases) {
1170 res.add_curve_ptr(phase.m_ddc);
1171 }
1172 return res;
1173 }
1174
1181 piecewise_t res = piecewise_t();
1182 for (const ContactPhase& phase : m_contact_phases) {
1183 res.add_curve_ptr(phase.m_L);
1184 }
1185 return res;
1186 }
1187
1194 piecewise_t res = piecewise_t();
1195 for (const ContactPhase& phase : m_contact_phases) {
1196 res.add_curve_ptr(phase.m_dL);
1197 }
1198 return res;
1199 }
1200
1207 piecewise_t res = piecewise_t();
1208 for (const ContactPhase& phase : m_contact_phases) {
1209 res.add_curve_ptr(phase.m_zmp);
1210 }
1211 return res;
1212 }
1213
1221 piecewise_t res = piecewise_t();
1222 for (const ContactPhase& phase : m_contact_phases) {
1223 res.add_curve_ptr(phase.m_wrench);
1224 }
1225 return res;
1226 }
1227
1234 piecewise_t res = piecewise_t();
1235 for (const ContactPhase& phase : m_contact_phases) {
1236 res.add_curve_ptr(phase.m_q);
1237 }
1238 return res;
1239 }
1240
1247 piecewise_t res = piecewise_t();
1248 for (const ContactPhase& phase : m_contact_phases) {
1249 res.add_curve_ptr(phase.m_dq);
1250 }
1251 return res;
1252 }
1253
1260 piecewise_t res = piecewise_t();
1261 for (const ContactPhase& phase : m_contact_phases) {
1262 res.add_curve_ptr(phase.m_ddq);
1263 }
1264 return res;
1265 }
1266
1273 piecewise_t res = piecewise_t();
1274 for (const ContactPhase& phase : m_contact_phases) {
1275 res.add_curve_ptr(phase.m_tau);
1276 }
1277 return res;
1278 }
1279
1287 for (const ContactPhase& phase : m_contact_phases) {
1288 res.add_curve_ptr(phase.m_root);
1289 }
1290 return res;
1291 }
1292
1303 const std::string& eeName) const {
1305 transform_t last_placement, first_placement;
1306 // first find the first and last phase with a trajectory for this effector
1307 size_t first_phase = m_contact_phases.size();
1308 size_t last_phase = 0;
1309 for (size_t i = 0; i < m_contact_phases.size(); ++i) {
1310 if (m_contact_phases.at(i).effectorHaveAtrajectory(eeName)) {
1311 last_phase = i;
1312 if (first_phase > i) {
1313 first_phase = i;
1314 curve_SE3_ptr curve =
1315 m_contact_phases.at(i).effectorTrajectories().at(eeName);
1316 first_placement = curve->operator()(curve->min());
1317 }
1318 }
1319 }
1320 if (first_phase == m_contact_phases.size())
1321 throw std::invalid_argument(
1322 "The contact sequence doesn't have any phase with an effector "
1323 "trajectory"
1324 " for the given effector name");
1325 if (first_phase > 0) {
1326 // add a first constant phase at the initial placement
1327 curve_SE3_ptr ptr_init(
1328 new SE3Curve_t(first_placement, first_placement,
1329 m_contact_phases.at(0).timeInitial(),
1330 m_contact_phases.at(first_phase).timeInitial()));
1331 res.add_curve_ptr(ptr_init);
1332 }
1333 // loop over this phases to concatenate the trajectories
1334 for (size_t i = first_phase; i <= last_phase; ++i) {
1335 if (m_contact_phases.at(i).effectorHaveAtrajectory(eeName)) {
1336 res.add_curve_ptr(
1337 m_contact_phases.at(i).effectorTrajectories().at(eeName));
1338 last_placement = res(res.max());
1339 } else {
1340 // when the trajectory do not exist, add a constant one with the
1341 // previous value
1342 curve_SE3_ptr ptr(new SE3Curve_t(last_placement, last_placement,
1343 m_contact_phases.at(i).timeInitial(),
1344 m_contact_phases.at(i).timeFinal()));
1345 res.add_curve_ptr(ptr);
1346 }
1347 }
1348 if (last_phase < m_contact_phases.size() - 1) {
1349 // add a last constant phase until the end of the contact sequence
1350 curve_SE3_ptr ptr_final(
1351 new SE3Curve_t(last_placement, last_placement,
1352 m_contact_phases.at(last_phase).timeFinal(),
1353 m_contact_phases.back().timeFinal()));
1354 res.add_curve_ptr(ptr_final);
1355 }
1356 return res;
1357 }
1358
1369 const std::string& eeName) const {
1370 piecewise_t res = piecewise_t();
1371 // first find the dimension used, in order to fill with 0 when required:
1372 size_t dim = 0;
1373 for (const ContactPhase& phase : m_contact_phases) {
1374 if (phase.contactForces().count(eeName) > 0) {
1375 dim = phase.contactForces().at(eeName)->dim();
1376 }
1377 }
1378 if (dim == 0) {
1379 // no forces trajectory for this effector
1380 return res;
1381 }
1382 Eigen::MatrixXd zeros = Eigen::MatrixXd::Zero(dim, 1);
1383 // loop over all phases and either add trajectory if it exist or add zeros
1384 for (const ContactPhase& phase : m_contact_phases) {
1385 if (phase.contactForces().count(eeName) > 0) {
1386 res.add_curve_ptr(phase.contactForces().at(eeName));
1387 } else {
1388 curve_ptr ptr(
1389 new polynomial_t(zeros, phase.timeInitial(), phase.timeFinal()));
1390 res.add_curve_ptr(ptr);
1391 }
1392 }
1393 return res;
1394 }
1395
1406 const std::string& eeName) const {
1407 piecewise_t res = piecewise_t();
1408 Eigen::MatrixXd zeros = Eigen::MatrixXd::Zero(1, 1);
1409 // loop over all phases and either add trajectory if it exist or add zeros
1410 for (const ContactPhase& phase : m_contact_phases) {
1411 if (phase.contactNormalForces().count(eeName) > 0) {
1412 res.add_curve_ptr(phase.contactNormalForces().at(eeName));
1413 } else {
1414 curve_ptr ptr(
1415 new polynomial_t(zeros, phase.timeInitial(), phase.timeFinal()));
1416 res.add_curve_ptr(ptr);
1417 }
1418 }
1419 return res;
1420 }
1421
1430 ContactPhase& phaseAtTime(const double time) {
1431 const int id = phaseIdAtTime(time);
1432 if (id >= 0)
1433 return m_contact_phases.at(id);
1434 else
1435 throw std::invalid_argument("No phase found for the given time.");
1436 }
1437
1446 int phaseIdAtTime(const double time) const {
1447 for (int i = 0; i < static_cast<int>(m_contact_phases.size()); ++i) {
1448 const ContactPhase& phase = m_contact_phases[i];
1449 if (time >= phase.timeInitial() && time < phase.timeFinal()) {
1450 return i;
1451 }
1452 }
1453 if (time == m_contact_phases.back().timeFinal())
1454 return static_cast<int>(m_contact_phases.size() - 1);
1455 return -1;
1456 }
1457
1458 /* End Helpers */
1459
1460 /*Public Attributes*/
1462 /*Public Attributes*/
1463
1464 private:
1465 // Serialization of the class
1467
1468 template <class Archive>
1469 void save(Archive& ar, const unsigned int /*version*/) const {
1470 const size_t m_size = size();
1471 ar& boost::serialization::make_nvp("size", m_size);
1472 for (typename ContactPhaseVector::const_iterator it =
1473 m_contact_phases.begin();
1474 it != m_contact_phases.end(); ++it) {
1475 ar& boost::serialization::make_nvp("contact_phase", *it);
1476 }
1477 }
1478
1479 template <class Archive>
1480 void load(Archive& ar, const unsigned int /*version*/) {
1481 size_t m_size;
1482 ar >> boost::serialization::make_nvp("size", m_size);
1483 assert(m_size > 0);
1484 resize(m_size);
1485 for (typename ContactPhaseVector::iterator it = m_contact_phases.begin();
1486 it != m_contact_phases.end(); ++it) {
1487 ar >> boost::serialization::make_nvp("contact_phase", *it);
1488 }
1489 }
1490
1491 BOOST_SERIALIZATION_SPLIT_MEMBER()
1492
1493}; // end class ContactSequence
1494
1495} // namespace scenario
1496} // namespace multicontact_api
1497
1500
1501#endif // __multicontact_api_scenario_contact_sequence_hpp__
@ CONTACT_UNDEFINED
Definition: fwd.hpp:33
Definition: ellipsoid.hpp:12
#define DEFINE_CLASS_TEMPLATE_VERSION(Template, Type)
Definition: archive.hpp:22
Definition: contact-patch.hpp:17
const SE3 & placement() const
Definition: contact-patch.hpp:45
pinocchio::SE3Tpl< Scalar, 0 > SE3
Definition: contact-patch.hpp:22
Definition: contact-phase.hpp:29
point3_t m_L_final
Final angular momentum for this contact phase.
Definition: contact-phase.hpp:243
Scalar timeFinal() const
Definition: contact-phase.hpp:277
ContactPatch & contactPatch(const std::string &eeName)
Definition: contact-phase.hpp:394
bool addContact(const std::string &eeName, const ContactPatch &patch)
addContact add a new contact patch to this contact phase If a contact phase already exist for this ef...
Definition: contact-phase.hpp:413
ContactPatch::SE3 SE3
Definition: contact-phase.hpp:55
point3_t m_dL_final
Final angular momentum derivative for this contact phase.
Definition: contact-phase.hpp:245
point3_t m_ddc_final
Final acceleration of the center of mass for this contact phase.
Definition: contact-phase.hpp:241
bool isEffectorInContact(const std::string &eeName) const
Definition: contact-phase.hpp:447
point3_t m_dL_init
Initial angular momentum derivative for this contact phase.
Definition: contact-phase.hpp:233
point3_t m_dc_init
Initial velocity of the center of mass for this contact phase.
Definition: contact-phase.hpp:227
Scalar timeInitial() const
Definition: contact-phase.hpp:275
point3_t m_L_init
Initial angular momentum for this contact phase.
Definition: contact-phase.hpp:231
pointX_t m_q_final
Final whole body configuration of this phase.
Definition: contact-phase.hpp:247
point3_t m_c_init
Initial position of the center of mass for this contact phase.
Definition: contact-phase.hpp:225
point3_t m_dc_final
Final velocity of the center of mass for this contact phase.
Definition: contact-phase.hpp:239
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef _Scalar Scalar
Definition: contact-phase.hpp:32
std::vector< std::string > t_strings
Definition: contact-phase.hpp:53
t_strings effectorsInContact() const
Definition: contact-phase.hpp:445
ndcurves::curve_SE3_ptr_t curve_SE3_ptr
Definition: contact-phase.hpp:48
Scalar duration() const
Definition: contact-phase.hpp:283
point3_t m_c_final
Final position of the center of mass for this contact phase.
Definition: contact-phase.hpp:237
point3_t m_ddc_init
Initial acceleration of the center of mass for this contact phase.
Definition: contact-phase.hpp:229
pointX_t m_q_init
Initial whole body configuration of this phase.
Definition: contact-phase.hpp:235
Definition: contact-sequence.hpp:22
piecewise_t concatenateDCtrajectories() const
concatenateDCtrajectories Return a piecewise curve which is the concatenation of the m_dc curves for ...
Definition: contact-sequence.hpp:1154
ContactPhase & phaseAtTime(const double time)
phaseAtTime return a phase of the sequence such that : phase.timeInitial <= t < phase....
Definition: contact-sequence.hpp:1430
piecewise_t concatenateZMPtrajectories() const
concatenateZMPtrajectories Return a piecewise curve which is the concatenation of the m_zmp curves fo...
Definition: contact-sequence.hpp:1206
ContactPhase::Scalar Scalar
Definition: contact-sequence.hpp:25
ContactPhaseVector m_contact_phases
Definition: contact-sequence.hpp:1461
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef _ContactPhase ContactPhase
Definition: contact-sequence.hpp:24
piecewise_t concatenateDDQtrajectories() const
concatenateDDQtrajectories Return a piecewise curve which is the concatenation of the m_ddq curves fo...
Definition: contact-sequence.hpp:1259
bool haveAMvalues() const
haveAMvalues Check that the initial and final AM values are defined for all phases Also check that th...
Definition: contact-sequence.hpp:414
piecewise_t concatenateCtrajectories() const
concatenateCtrajectories Return a piecewise curve which is the concatenation of the m_c curves for ea...
Definition: contact-sequence.hpp:1141
ndcurves::curve_SE3_t curve_SE3_t
Definition: contact-sequence.hpp:33
ContactPhase & contactPhase(const size_t id)
contactPhase return a reference to the contactPhase stored at the given Id
Definition: contact-sequence.hpp:85
ndcurves::transform_t transform_t
Definition: contact-sequence.hpp:31
piecewise_t concatenateDLtrajectories() const
concatenateDLtrajectories Return a piecewise curve which is the concatenation of the m_dL curves for ...
Definition: contact-sequence.hpp:1193
bool haveCentroidalValues() const
haveCentroidalValues Check that the initial and final CoM position and AM values are defined for all ...
Definition: contact-sequence.hpp:440
const ContactPhaseVector contactPhases() const
contactPhases return a Const copy of the contact phase vector in this sequence. Prefer accessing the ...
Definition: contact-sequence.hpp:77
ndcurves::polynomial_t polynomial_t
Definition: contact-sequence.hpp:39
bool haveCOMtrajectories() const
haveCOMtrajectories check that a c, dc and ddc trajectories are defined for each phases Also check th...
Definition: contact-sequence.hpp:483
int phaseIdAtTime(const double time) const
phaseIdAtTime return the index of a phase in the sequence such that : phase.timeInitial <= t < phase....
Definition: contact-sequence.hpp:1446
piecewise_SE3_t concatenateRootTrajectories() const
concatenateRootTrajectories Return a piecewise curve which is the concatenation of the m_root curves ...
Definition: contact-sequence.hpp:1285
bool haveConfigurationsValues() const
haveConfigurationsValues Check that the initial and final configuration are defined for all phases Al...
Definition: contact-sequence.hpp:450
ndcurves::SE3Curve_t SE3Curve_t
Definition: contact-sequence.hpp:38
bool haveJointsTrajectories() const
haveJointsTrajectories Check that a q trajectory is defined for each phases Also check that the time ...
Definition: contact-sequence.hpp:849
piecewise_t concatenateDDCtrajectories() const
concatenateDDCtrajectories Return a piecewise curve which is the concatenation of the m_ddc curves fo...
Definition: contact-sequence.hpp:1167
ndcurves::curve_abc_t curve_t
Definition: contact-sequence.hpp:32
piecewise_t concatenateTauTrajectories() const
concatenateTauTrajectories Return a piecewise curve which is the concatenation of the m_tau curves fo...
Definition: contact-sequence.hpp:1272
void resize(const size_t size)
Definition: contact-sequence.hpp:57
ContactSequenceTpl(const ContactSequenceTpl &other)
Copy contructor.
Definition: contact-sequence.hpp:44
size_t size() const
Definition: contact-sequence.hpp:47
piecewise_t concatenateLtrajectories() const
concatenateLtrajectories Return a piecewise curve which is the concatenation of the m_L curves for ea...
Definition: contact-sequence.hpp:1180
void removePhase(const size_t id)
removePhase remove the given contactPhase from the sequence
Definition: contact-sequence.hpp:97
bool haveCOMvalues() const
haveCOMvalues Check that the initial and final CoM position values are defined for all phases Also ch...
Definition: contact-sequence.hpp:364
bool haveContactModelDefined() const
haveContactModelDefined check that all the contact patch have a contact_model defined
Definition: contact-sequence.hpp:1070
ndcurves::piecewise_SE3_t piecewise_SE3_t
Definition: contact-sequence.hpp:37
bool haveJointsDerivativesTrajectories() const
haveJointsDerivativesTrajectories Check that a dq and ddq trajectories are defined for each phases Al...
Definition: contact-sequence.hpp:890
piecewise_t concatenateQtrajectories() const
concatenateQtrajectories Return a piecewise curve which is the concatenation of the m_q curves for ea...
Definition: contact-sequence.hpp:1233
void moveEffectorOf(const std::string &eeName, const ContactPatch::SE3 &transform, const double durationBreak=-1, const double durationCreate=-1)
moveEffectorOf similar to moveEffectorToPlacement exept that the new placement is defined from the pr...
Definition: contact-sequence.hpp:265
bool operator!=(const ContactSequenceTpl &other) const
Definition: contact-sequence.hpp:53
boost::shared_ptr< curve_SE3_t > curve_SE3_ptr
Definition: contact-sequence.hpp:35
std::vector< ContactPhase > ContactPhaseVector
Definition: contact-sequence.hpp:28
bool haveConsistentContacts() const
haveConsistentContacts check that there is always one contact change between adjacent phases in the s...
Definition: contact-sequence.hpp:317
bool haveTorquesTrajectories() const
haveJointsTrajectories Check that a joint torque trajectories are defined for each phases Also check ...
Definition: contact-sequence.hpp:941
bool haveTimings() const
haveTimings Check if all the time intervals are defined and consistent (ie. the time always increase ...
Definition: contact-sequence.hpp:287
void createContact(const std::string &eeName, const ContactPatch &patch, const double phaseDuration=-1)
createContact Add a new contactPhase at the end of the current ContactSequence, The new ContactPhase ...
Definition: contact-sequence.hpp:171
ContactPhase::SE3 SE3
Definition: contact-sequence.hpp:27
bool haveZMPtrajectories()
haveZMPtrajectories check that all the contact phases have a zmp trajectory
Definition: contact-sequence.hpp:1091
friend class boost::serialization::access
Definition: contact-sequence.hpp:1466
piecewise_t concatenateContactForceTrajectories(const std::string &eeName) const
concatenateContactForceTrajectories Return a piecewise curve which is the concatenation of the contac...
Definition: contact-sequence.hpp:1368
bool haveContactForcesTrajectories() const
haveJointsTrajectories Check that a contact force trajectory exist for each active contact Also check...
Definition: contact-sequence.hpp:973
boost::shared_ptr< curve_t > curve_ptr
Definition: contact-sequence.hpp:34
t_strings getAllEffectorsInContact() const
getAllEffectorsInContact return a vector of names of all the effectors used to create contacts during...
Definition: contact-sequence.hpp:1124
void moveEffectorToPlacement(const std::string &eeName, const ContactPatch::SE3 &placement, const double durationBreak=-1, const double durationCreate=-1)
moveEffectorToPlacement Add two new phases at the end of the current ContactSequence,
Definition: contact-sequence.hpp:225
ContactSequenceTpl(const size_t size=0)
Definition: contact-sequence.hpp:41
piecewise_t concatenateDQtrajectories() const
concatenateDQtrajectories Return a piecewise curve which is the concatenation of the m_dq curves for ...
Definition: contact-sequence.hpp:1246
bool haveFriction() const
haveFriction check that all the contact patch used in the sequence have a friction coefficient initia...
Definition: contact-sequence.hpp:1050
piecewise_t concatenateNormalForceTrajectories(const std::string &eeName) const
concatenateNormalForceTrajectories Return a piecewise curve which is the concatenation of the contact...
Definition: contact-sequence.hpp:1405
void breakContact(const std::string &eeName, const double phaseDuration=-1)
breakContact Add a new contactPhase at the end of the current ContactSequence, The new ContactPhase h...
Definition: contact-sequence.hpp:121
ndcurves::piecewise_t piecewise_t
Definition: contact-sequence.hpp:36
ndcurves::pointX_t pointX_t
Definition: contact-sequence.hpp:30
piecewise_t concatenateWrenchTrajectories() const
concatenateWrenchTrajectories Return a piecewise curve which is the concatenation of the m_wrench cur...
Definition: contact-sequence.hpp:1220
bool haveCentroidalTrajectories() const
haveCentroidalTrajectories check that all centroidal trajectories are defined for each phases Also ch...
Definition: contact-sequence.hpp:742
ContactPhase::t_strings t_strings
Definition: contact-sequence.hpp:26
bool operator==(const ContactSequenceTpl &other) const
Definition: contact-sequence.hpp:49
bool haveAMtrajectories() const
haveAMtrajectories check that a L and dL trajectories are defined for each phases Also check that the...
Definition: contact-sequence.hpp:633
size_t append(const ContactPhase &contactPhase)
append Add the given Phase at the end of the sequence
Definition: contact-sequence.hpp:66
bool haveRootTrajectories() const
haveRootTrajectories check that a root trajectory exist for each contact phases. Also check that it s...
Definition: contact-sequence.hpp:1022
piecewise_SE3_t concatenateEffectorTrajectories(const std::string &eeName) const
concatenateEffectorTrajectories Return a piecewise curve which is the concatenation of the effectors ...
Definition: contact-sequence.hpp:1302
bool haveEffectorsTrajectories(const Scalar prec=Eigen::NumTraits< Scalar >::dummy_precision(), const bool use_rotation=true) const
haveEffectorsTrajectories check that for each phase preceeding a contact creation,...
Definition: contact-sequence.hpp:756