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 
17 namespace multicontact_api {
18 namespace scenario {
19 
20 template <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 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  ContactSequenceTpl(const size_t size = 0) : m_contact_phases(size) {}
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 
66  size_t append(const ContactPhase& contactPhase) {
67  m_contact_phases.push_back(contactPhase);
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 
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 
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 pTrajMax =
782  ContactPatch::SE3((*traj)(traj->max()).matrix());
783  ContactPatch::SE3 pPhaseMax =
784  m_contact_phases.at(i + 1).contactPatches().at(eeName).placement();
785  if ((use_rotation &&
786  !(pTrajMax.toHomogeneousMatrix() - pPhaseMax.toHomogeneousMatrix())
787  .isMuchSmallerThan(1.0, prec)) ||
788  !(pTrajMax.translation() - pPhaseMax.translation())
789  .isMuchSmallerThan(1.0, prec)) {
790  std::cout << "Effector trajectory for " << eeName
791  << " do not end at it's contact placement in the next "
792  "phase, for phase "
793  << i << std::endl;
794  std::cout << "Last point : " << std::endl
795  << pTrajMax << std::endl
796  << "Next contact : " << std::endl
797  << pPhaseMax << std::endl;
798  return false;
799  }
800  if (i > 0 && m_contact_phases.at(i - 1).isEffectorInContact(eeName)) {
801  ContactPatch::SE3 pTrajMin =
802  ContactPatch::SE3((*traj)(traj->min()).matrix());
803  ContactPatch::SE3 pPhaseMin = m_contact_phases.at(i - 1)
804  .contactPatches()
805  .at(eeName)
806  .placement();
807  if ((use_rotation && !(pTrajMin.toHomogeneousMatrix() -
808  pPhaseMin.toHomogeneousMatrix())
809  .isMuchSmallerThan(1.0, prec)) ||
810  !(pTrajMin.translation() - pPhaseMin.translation())
811  .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  return true;
826  }
827 
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 
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 
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 
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 
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 
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 
1057  size_t i = 0;
1058  for (const ContactPhase& phase : m_contact_phases) {
1059  for (const std::string& eeName : phase.effectorsInContact()) {
1060  if (phase.contactPatches().at(eeName).m_contact_model.m_contact_type ==
1062  std::cout << "ContactModel not defined for phase " << i
1063  << " and effector " << eeName << std::endl;
1064  return false;
1065  }
1066  }
1067  ++i;
1068  }
1069  return true;
1070  }
1071 
1078  size_t i = 0;
1079  for (const ContactPhase& phase : m_contact_phases) {
1080  if (!phase.m_zmp) {
1081  std::cout << "ZMP trajectory not defined for phase : " << i
1082  << std::endl;
1083  return false;
1084  }
1085  if (phase.m_zmp->dim() != 3) {
1086  std::cout << "ZMP trajectory is not of dimension 3 for phase : " << i
1087  << std::endl;
1088  return false;
1089  }
1090  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  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  ++i;
1101  }
1102  return true;
1103  }
1104 
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 
1128  piecewise_t res = piecewise_t();
1129  for (const ContactPhase& phase : m_contact_phases) {
1130  res.add_curve_ptr(phase.m_c);
1131  }
1132  return res;
1133  }
1134 
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 
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 
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 
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 
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 
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 
1220  piecewise_t res = piecewise_t();
1221  for (const ContactPhase& phase : m_contact_phases) {
1222  res.add_curve_ptr(phase.m_q);
1223  }
1224  return res;
1225  }
1226 
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 
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 
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 
1273  for (const ContactPhase& phase : m_contact_phases) {
1274  res.add_curve_ptr(phase.m_root);
1275  }
1276  return res;
1277  }
1278 
1289  const std::string& eeName) const {
1291  transform_t last_placement, first_placement;
1292  // first find the first and last phase with a trajectory for this effector
1293  size_t first_phase = m_contact_phases.size();
1294  size_t last_phase = 0;
1295  for (size_t i = 0; i < m_contact_phases.size(); ++i) {
1296  if (m_contact_phases.at(i).effectorHaveAtrajectory(eeName)) {
1297  last_phase = i;
1298  if (first_phase > i) {
1299  first_phase = i;
1300  curve_SE3_ptr curve =
1301  m_contact_phases.at(i).effectorTrajectories().at(eeName);
1302  first_placement = curve->operator()(curve->min());
1303  }
1304  }
1305  }
1306  if (first_phase == m_contact_phases.size())
1307  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  if (first_phase > 0) {
1312  // add a first constant phase at the initial placement
1313  curve_SE3_ptr ptr_init(
1314  new SE3Curve_t(first_placement, first_placement,
1315  m_contact_phases.at(0).timeInitial(),
1316  m_contact_phases.at(first_phase).timeInitial()));
1317  res.add_curve_ptr(ptr_init);
1318  }
1319  // loop over this phases to concatenate the trajectories
1320  for (size_t i = first_phase; i <= last_phase; ++i) {
1321  if (m_contact_phases.at(i).effectorHaveAtrajectory(eeName)) {
1322  res.add_curve_ptr(
1323  m_contact_phases.at(i).effectorTrajectories().at(eeName));
1324  last_placement = res(res.max());
1325  } else {
1326  // when the trajectory do not exist, add a constant one with the
1327  // previous value
1328  curve_SE3_ptr ptr(new SE3Curve_t(last_placement, last_placement,
1329  m_contact_phases.at(i).timeInitial(),
1330  m_contact_phases.at(i).timeFinal()));
1331  res.add_curve_ptr(ptr);
1332  }
1333  }
1334  if (last_phase < m_contact_phases.size() - 1) {
1335  // add a last constant phase until the end of the contact sequence
1336  curve_SE3_ptr ptr_final(
1337  new SE3Curve_t(last_placement, last_placement,
1338  m_contact_phases.at(last_phase).timeFinal(),
1339  m_contact_phases.back().timeFinal()));
1340  res.add_curve_ptr(ptr_final);
1341  }
1342  return res;
1343  }
1344 
1355  const std::string& eeName) const {
1356  piecewise_t res = piecewise_t();
1357  // first find the dimension used, in order to fill with 0 when required:
1358  size_t dim = 0;
1359  for (const ContactPhase& phase : m_contact_phases) {
1360  if (phase.contactForces().count(eeName) > 0) {
1361  dim = phase.contactForces().at(eeName)->dim();
1362  }
1363  }
1364  if (dim == 0) {
1365  // no forces trajectory for this effector
1366  return res;
1367  }
1368  Eigen::MatrixXd zeros = Eigen::MatrixXd::Zero(dim, 1);
1369  // loop over all phases and either add trajectory if it exist or add zeros
1370  for (const ContactPhase& phase : m_contact_phases) {
1371  if (phase.contactForces().count(eeName) > 0) {
1372  res.add_curve_ptr(phase.contactForces().at(eeName));
1373  } else {
1374  curve_ptr ptr(
1375  new polynomial_t(zeros, phase.timeInitial(), phase.timeFinal()));
1376  res.add_curve_ptr(ptr);
1377  }
1378  }
1379  return res;
1380  }
1381 
1392  const std::string& eeName) const {
1393  piecewise_t res = piecewise_t();
1394  Eigen::MatrixXd zeros = Eigen::MatrixXd::Zero(1, 1);
1395  // loop over all phases and either add trajectory if it exist or add zeros
1396  for (const ContactPhase& phase : m_contact_phases) {
1397  if (phase.contactNormalForces().count(eeName) > 0) {
1398  res.add_curve_ptr(phase.contactNormalForces().at(eeName));
1399  } else {
1400  curve_ptr ptr(
1401  new polynomial_t(zeros, phase.timeInitial(), phase.timeFinal()));
1402  res.add_curve_ptr(ptr);
1403  }
1404  }
1405  return res;
1406  }
1407 
1416  ContactPhase& phaseAtTime(const double time) {
1417  const int id = phaseIdAtTime(time);
1418  if (id >= 0)
1419  return m_contact_phases.at(id);
1420  else
1421  throw std::invalid_argument("No phase found for the given time.");
1422  }
1423 
1432  int phaseIdAtTime(const double time) const {
1433  for (int i = 0; i < static_cast<int>(m_contact_phases.size()); ++i) {
1434  const ContactPhase& phase = m_contact_phases[i];
1435  if (time >= phase.timeInitial() && time < phase.timeFinal()) {
1436  return i;
1437  }
1438  }
1439  if (time == m_contact_phases.back().timeFinal())
1440  return static_cast<int>(m_contact_phases.size() - 1);
1441  return -1;
1442  }
1443 
1444  /* End Helpers */
1445 
1446  /*Public Attributes*/
1448  /*Public Attributes*/
1449 
1450  private:
1451  // Serialization of the class
1453 
1454  template <class Archive>
1455  void save(Archive& ar, const unsigned int /*version*/) const {
1456  const size_t m_size = size();
1457  ar& boost::serialization::make_nvp("size", m_size);
1458  for (typename ContactPhaseVector::const_iterator it =
1459  m_contact_phases.begin();
1460  it != m_contact_phases.end(); ++it) {
1461  ar& boost::serialization::make_nvp("contact_phase", *it);
1462  }
1463  }
1464 
1465  template <class Archive>
1466  void load(Archive& ar, const unsigned int /*version*/) {
1467  size_t m_size;
1468  ar >> boost::serialization::make_nvp("size", m_size);
1469  assert(m_size > 0);
1470  resize(m_size);
1471  for (typename ContactPhaseVector::iterator it = m_contact_phases.begin();
1472  it != m_contact_phases.end(); ++it) {
1473  ar >> boost::serialization::make_nvp("contact_phase", *it);
1474  }
1475  }
1476 
1477  BOOST_SERIALIZATION_SPLIT_MEMBER()
1478 
1479 }; // end class ContactSequence
1480 
1481 } // namespace scenario
1482 } // namespace multicontact_api
1483 
1486 
1487 #endif // __multicontact_api_scenario_contact_sequence_hpp__
multicontact_api::scenario::ContactSequenceTpl::concatenateQtrajectories
piecewise_t concatenateQtrajectories() const
concatenateQtrajectories Return a piecewise curve which is the concatenation of the m_q curves for ea...
Definition: contact-sequence.hpp:1219
multicontact_api::scenario::ContactPhaseTpl::m_ddc_init
point3_t m_ddc_init
Initial acceleration of the center of mass for this contact phase.
Definition: contact-phase.hpp:229
multicontact_api::scenario::ContactSequenceTpl::concatenateTauTrajectories
piecewise_t concatenateTauTrajectories() const
concatenateTauTrajectories Return a piecewise curve which is the concatenation of the m_tau curves fo...
Definition: contact-sequence.hpp:1258
multicontact_api::scenario::ContactPhaseTpl::m_ddc_final
point3_t m_ddc_final
Final acceleration of the center of mass for this contact phase.
Definition: contact-phase.hpp:241
multicontact_api::scenario::ContactSequenceTpl::concatenateLtrajectories
piecewise_t concatenateLtrajectories() const
concatenateLtrajectories Return a piecewise curve which is the concatenation of the m_L curves for ea...
Definition: contact-sequence.hpp:1166
multicontact_api::scenario::ContactSequenceTpl::concatenateRootTrajectories
piecewise_SE3_t concatenateRootTrajectories() const
concatenateRootTrajectories Return a piecewise curve which is the concatenation of the m_root curves ...
Definition: contact-sequence.hpp:1271
multicontact_api::scenario::ContactSequenceTpl::removePhase
void removePhase(const size_t id)
removePhase remove the given contactPhase from the sequence
Definition: contact-sequence.hpp:97
multicontact_api::scenario::ContactPhaseTpl::curve_SE3_ptr
ndcurves::curve_SE3_ptr_t curve_SE3_ptr
Definition: contact-phase.hpp:48
multicontact_api::scenario::ContactSequenceTpl::curve_t
ndcurves::curve_abc_t curve_t
Definition: contact-sequence.hpp:32
multicontact_api::scenario::ContactSequenceTpl::moveEffectorOf
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
multicontact_api::scenario::ContactPhaseTpl::effectorsInContact
t_strings effectorsInContact() const
Definition: contact-phase.hpp:445
multicontact_api::scenario::ContactSequenceTpl::concatenateDLtrajectories
piecewise_t concatenateDLtrajectories() const
concatenateDLtrajectories Return a piecewise curve which is the concatenation of the m_dL curves for ...
Definition: contact-sequence.hpp:1179
multicontact_api::scenario::ContactSequenceTpl::haveContactForcesTrajectories
bool haveContactForcesTrajectories() const
haveJointsTrajectories Check that a contact force trajectory exist for each active contact Also check...
Definition: contact-sequence.hpp:959
multicontact_api::scenario::ContactSequenceTpl::polynomial_t
ndcurves::polynomial_t polynomial_t
Definition: contact-sequence.hpp:39
multicontact_api::scenario::ContactSequenceTpl::breakContact
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
multicontact_api::scenario::ContactSequenceTpl::curve_SE3_ptr
std::shared_ptr< curve_SE3_t > curve_SE3_ptr
Definition: contact-sequence.hpp:35
multicontact_api::scenario::ContactSequenceTpl::piecewise_t
ndcurves::piecewise_t piecewise_t
Definition: contact-sequence.hpp:36
multicontact_api::scenario::ContactSequenceTpl::haveEffectorsTrajectories
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
multicontact_api::scenario::ContactSequenceTpl::ContactPhase
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef _ContactPhase ContactPhase
Definition: contact-sequence.hpp:24
archive.hpp
multicontact_api::scenario::ContactSequenceTpl::haveRootTrajectories
bool haveRootTrajectories() const
haveRootTrajectories check that a root trajectory exist for each contact phases. Also check that it s...
Definition: contact-sequence.hpp:1008
multicontact_api::scenario::ContactSequenceTpl::haveConsistentContacts
bool haveConsistentContacts() const
haveConsistentContacts check that there is always one contact change between adjacent phases in the s...
Definition: contact-sequence.hpp:317
fwd.hpp
multicontact_api::scenario::ContactSequenceTpl::haveTimings
bool haveTimings() const
haveTimings Check if all the time intervals are defined and consistent (ie. the time always increase ...
Definition: contact-sequence.hpp:287
multicontact_api::scenario::ContactPhaseTpl::m_q_init
pointX_t m_q_init
Initial whole body configuration of this phase.
Definition: contact-phase.hpp:235
multicontact_api::scenario::ContactSequenceTpl::contactPhase
ContactPhase & contactPhase(const size_t id)
contactPhase return a reference to the contactPhase stored at the given Id
Definition: contact-sequence.hpp:85
multicontact_api::scenario::ContactSequenceTpl::haveTorquesTrajectories
bool haveTorquesTrajectories() const
haveJointsTrajectories Check that a joint torque trajectories are defined for each phases Also check ...
Definition: contact-sequence.hpp:927
multicontact_api::scenario::ContactSequenceTpl::haveJointsTrajectories
bool haveJointsTrajectories() const
haveJointsTrajectories Check that a q trajectory is defined for each phases Also check that the time ...
Definition: contact-sequence.hpp:835
multicontact_api::scenario::ContactPatchTpl
Definition: contact-patch.hpp:16
multicontact_api::scenario::ContactSequenceTpl::ContactPhaseVector
std::vector< ContactPhase > ContactPhaseVector
Definition: contact-sequence.hpp:28
multicontact_api::scenario::ContactSequenceTpl::operator==
bool operator==(const ContactSequenceTpl &other) const
Definition: contact-sequence.hpp:49
multicontact_api::scenario::ContactSequenceTpl::curve_ptr
std::shared_ptr< curve_t > curve_ptr
Definition: contact-sequence.hpp:34
multicontact_api::scenario::ContactSequenceTpl::t_strings
ContactPhase::t_strings t_strings
Definition: contact-sequence.hpp:26
multicontact_api::scenario::ContactSequenceTpl::piecewise_SE3_t
ndcurves::piecewise_SE3_t piecewise_SE3_t
Definition: contact-sequence.hpp:37
multicontact_api
Definition: ellipsoid.hpp:12
multicontact_api::scenario::ContactSequenceTpl::SE3
ContactPhase::SE3 SE3
Definition: contact-sequence.hpp:27
multicontact_api::scenario::ContactSequenceTpl::access
friend class boost::serialization::access
Definition: contact-sequence.hpp:1452
multicontact_api::scenario::ContactSequenceTpl::haveJointsDerivativesTrajectories
bool haveJointsDerivativesTrajectories() const
haveJointsDerivativesTrajectories Check that a dq and ddq trajectories are defined for each phases Al...
Definition: contact-sequence.hpp:876
multicontact_api::scenario::ContactPhaseTpl::m_L_init
point3_t m_L_init
Initial angular momentum for this contact phase.
Definition: contact-phase.hpp:231
multicontact_api::scenario::ContactSequenceTpl::concatenateZMPtrajectories
piecewise_t concatenateZMPtrajectories() const
concatenateZMPtrajectories Return a piecewise curve which is the concatenation of the m_zmp curves fo...
Definition: contact-sequence.hpp:1192
multicontact_api::scenario::ContactPatchTpl::SE3
pinocchio::SE3Tpl< Scalar, 0 > SE3
Definition: contact-patch.hpp:22
multicontact_api::scenario::ContactSequenceTpl::concatenateContactForceTrajectories
piecewise_t concatenateContactForceTrajectories(const std::string &eeName) const
concatenateContactForceTrajectories Return a piecewise curve which is the concatenation of the contac...
Definition: contact-sequence.hpp:1354
multicontact_api::scenario::ContactSequenceTpl::m_contact_phases
ContactPhaseVector m_contact_phases
Definition: contact-sequence.hpp:1447
multicontact_api::scenario::ContactPhaseTpl::m_L_final
point3_t m_L_final
Final angular momentum for this contact phase.
Definition: contact-phase.hpp:243
MULTICONTACT_API_DEFINE_CLASS_TEMPLATE_VERSION
#define MULTICONTACT_API_DEFINE_CLASS_TEMPLATE_VERSION(Template, Type)
Definition: archive.hpp:22
multicontact_api::scenario::CONTACT_UNDEFINED
@ CONTACT_UNDEFINED
Definition: fwd.hpp:33
multicontact_api::scenario::ContactSequenceTpl::haveConfigurationsValues
bool haveConfigurationsValues() const
haveConfigurationsValues Check that the initial and final configuration are defined for all phases Al...
Definition: contact-sequence.hpp:450
multicontact_api::scenario::ContactSequenceTpl::concatenateDQtrajectories
piecewise_t concatenateDQtrajectories() const
concatenateDQtrajectories Return a piecewise curve which is the concatenation of the m_dq curves for ...
Definition: contact-sequence.hpp:1232
multicontact_api::scenario::ContactSequenceTpl::concatenateCtrajectories
piecewise_t concatenateCtrajectories() const
concatenateCtrajectories Return a piecewise curve which is the concatenation of the m_c curves for ea...
Definition: contact-sequence.hpp:1127
multicontact_api::scenario::ContactPhaseTpl::contactPatch
ContactPatch & contactPatch(const std::string &eeName)
Definition: contact-phase.hpp:394
multicontact_api::scenario::ContactSequenceTpl::moveEffectorToPlacement
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
multicontact_api::scenario::ContactSequenceTpl::haveZMPtrajectories
bool haveZMPtrajectories()
haveZMPtrajectories check that all the contact phases have a zmp trajectory
Definition: contact-sequence.hpp:1077
multicontact_api::scenario::ContactSequenceTpl::concatenateNormalForceTrajectories
piecewise_t concatenateNormalForceTrajectories(const std::string &eeName) const
concatenateNormalForceTrajectories Return a piecewise curve which is the concatenation of the contact...
Definition: contact-sequence.hpp:1391
multicontact_api::scenario::ContactPhaseTpl::m_q_final
pointX_t m_q_final
Final whole body configuration of this phase.
Definition: contact-phase.hpp:247
multicontact_api::scenario::ContactPhaseTpl::m_c_init
point3_t m_c_init
Initial position of the center of mass for this contact phase.
Definition: contact-phase.hpp:225
multicontact_api::scenario::ContactSequenceTpl::concatenateEffectorTrajectories
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:1288
multicontact_api::scenario::ContactSequenceTpl::contactPhases
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
multicontact_api::scenario::ContactSequenceTpl::concatenateDCtrajectories
piecewise_t concatenateDCtrajectories() const
concatenateDCtrajectories Return a piecewise curve which is the concatenation of the m_dc curves for ...
Definition: contact-sequence.hpp:1140
multicontact_api::scenario::ContactSequenceTpl::haveCOMvalues
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
multicontact_api::scenario::ContactSequenceTpl::haveCentroidalTrajectories
bool haveCentroidalTrajectories() const
haveCentroidalTrajectories check that all centroidal trajectories are defined for each phases Also ch...
Definition: contact-sequence.hpp:742
multicontact_api::scenario::ContactPhaseTpl::m_c_final
point3_t m_c_final
Final position of the center of mass for this contact phase.
Definition: contact-phase.hpp:237
multicontact_api::scenario::ContactSequenceTpl::ContactSequenceTpl
ContactSequenceTpl(const size_t size=0)
Definition: contact-sequence.hpp:41
multicontact_api::scenario::ContactSequenceTpl::transform_t
ndcurves::transform_t transform_t
Definition: contact-sequence.hpp:31
multicontact_api::scenario::ContactSequenceTpl::getAllEffectorsInContact
t_strings getAllEffectorsInContact() const
getAllEffectorsInContact return a vector of names of all the effectors used to create contacts during...
Definition: contact-sequence.hpp:1110
boost::serialization::save
void save(Archive &ar, const pinocchio::container::aligned_vector< T > &v, const unsigned int version)
Definition: aligned-vector.hpp:16
multicontact_api::scenario::ContactSequenceTpl::haveAMtrajectories
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
multicontact_api::scenario::ContactPhaseTpl::SE3
ContactPatch::SE3 SE3
Definition: contact-phase.hpp:55
multicontact_api::scenario::ContactSequenceTpl::pointX_t
ndcurves::pointX_t pointX_t
Definition: contact-sequence.hpp:30
multicontact_api::scenario::ContactSequenceTpl::SE3Curve_t
ndcurves::SE3Curve_t SE3Curve_t
Definition: contact-sequence.hpp:38
multicontact_api::scenario::ContactPhaseTpl::t_strings
std::vector< std::string > t_strings
Definition: contact-phase.hpp:53
multicontact_api::scenario::ContactSequenceTpl::phaseAtTime
ContactPhase & phaseAtTime(const double time)
phaseAtTime return a phase of the sequence such that : phase.timeInitial <= t < phase....
Definition: contact-sequence.hpp:1416
multicontact_api::scenario::ContactPhaseTpl::m_dL_final
point3_t m_dL_final
Final angular momentum derivative for this contact phase.
Definition: contact-phase.hpp:245
contact-phase.hpp
multicontact_api::scenario::ContactPhaseTpl::m_dc_final
point3_t m_dc_final
Final velocity of the center of mass for this contact phase.
Definition: contact-phase.hpp:239
boost::serialization::load
void load(Archive &ar, pinocchio::container::aligned_vector< T > &v, const unsigned int version)
Definition: aligned-vector.hpp:24
multicontact_api::scenario::ContactSequenceTpl::phaseIdAtTime
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:1432
multicontact_api::scenario::ContactSequenceTpl::size
size_t size() const
Definition: contact-sequence.hpp:47
multicontact_api::scenario::ContactPhaseTpl
Definition: contact-phase.hpp:28
multicontact_api::scenario::ContactSequenceTpl
Definition: contact-sequence.hpp:21
multicontact_api::scenario::ContactPhaseTpl::Scalar
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef _Scalar Scalar
Definition: contact-phase.hpp:32
multicontact_api::scenario::ContactSequenceTpl::append
size_t append(const ContactPhase &contactPhase)
append Add the given Phase at the end of the sequence
Definition: contact-sequence.hpp:66
multicontact_api::scenario::ContactSequenceTpl::haveFriction
bool haveFriction() const
haveFriction check that all the contact patch used in the sequence have a friction coefficient initia...
Definition: contact-sequence.hpp:1036
multicontact_api::scenario::ContactSequenceTpl::curve_SE3_t
ndcurves::curve_SE3_t curve_SE3_t
Definition: contact-sequence.hpp:33
multicontact_api::scenario::ContactSequenceTpl::createContact
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
multicontact_api::scenario::ContactSequenceTpl::concatenateDDQtrajectories
piecewise_t concatenateDDQtrajectories() const
concatenateDDQtrajectories Return a piecewise curve which is the concatenation of the m_ddq curves fo...
Definition: contact-sequence.hpp:1245
multicontact_api::scenario::ContactSequenceTpl::haveCOMtrajectories
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
multicontact_api::scenario::ContactSequenceTpl::ContactSequenceTpl
ContactSequenceTpl(const ContactSequenceTpl &other)
Copy contructor.
Definition: contact-sequence.hpp:44
multicontact_api::scenario::ContactSequenceTpl::haveAMvalues
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
multicontact_api::scenario::ContactPhaseTpl::timeFinal
Scalar timeFinal() const
Definition: contact-phase.hpp:277
multicontact_api::scenario::ContactSequenceTpl::Scalar
ContactPhase::Scalar Scalar
Definition: contact-sequence.hpp:25
multicontact_api::scenario::ContactSequenceTpl::haveContactModelDefined
bool haveContactModelDefined() const
haveContactModelDefined check that all the contact patch have a contact_model defined
Definition: contact-sequence.hpp:1056
multicontact_api::scenario::ContactPhaseTpl::isEffectorInContact
bool isEffectorInContact(const std::string &eeName) const
Definition: contact-phase.hpp:447
multicontact_api::serialization::Serializable
Definition: archive.hpp:38
multicontact_api::scenario::ContactPhaseTpl::timeInitial
Scalar timeInitial() const
Definition: contact-phase.hpp:275
multicontact_api::scenario::ContactSequenceTpl::haveCentroidalValues
bool haveCentroidalValues() const
haveCentroidalValues Check that the initial and final CoM position and AM values are defined for all ...
Definition: contact-sequence.hpp:440
multicontact_api::scenario::ContactSequenceTpl::resize
void resize(const size_t size)
Definition: contact-sequence.hpp:57
multicontact_api::scenario::ContactPhaseTpl::m_dc_init
point3_t m_dc_init
Initial velocity of the center of mass for this contact phase.
Definition: contact-phase.hpp:227
multicontact_api::scenario::ContactPhaseTpl::addContact
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
multicontact_api::scenario::ContactSequenceTpl::operator!=
bool operator!=(const ContactSequenceTpl &other) const
Definition: contact-sequence.hpp:53
multicontact_api::scenario::ContactPhaseTpl::duration
Scalar duration() const
Definition: contact-phase.hpp:283
multicontact_api::scenario::ContactSequenceTpl::concatenateWrenchTrajectories
piecewise_t concatenateWrenchTrajectories() const
concatenateWrenchTrajectories Return a piecewise curve which is the concatenation of the m_wrench cur...
Definition: contact-sequence.hpp:1206
multicontact_api::scenario::ContactSequenceTpl::concatenateDDCtrajectories
piecewise_t concatenateDDCtrajectories() const
concatenateDDCtrajectories Return a piecewise curve which is the concatenation of the m_ddc curves fo...
Definition: contact-sequence.hpp:1153
multicontact_api::scenario::ContactPhaseTpl::m_dL_init
point3_t m_dL_init
Initial angular momentum derivative for this contact phase.
Definition: contact-phase.hpp:233
multicontact_api::scenario::ContactPatchTpl::placement
const SE3 & placement() const
Definition: contact-patch.hpp:45