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 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 
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 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 
849  bool haveJointsTrajectories() const {
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 
941  bool haveTorquesTrajectories() const {
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 
1022  bool haveRootTrajectories() const {
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__
void load(Archive &ar, pinocchio::container::aligned_vector< T > &v, const unsigned int version)
Definition: aligned-vector.hpp:24
void save(Archive &ar, const pinocchio::container::aligned_vector< T > &v, const unsigned int version)
Definition: aligned-vector.hpp:16
@ 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
pinocchio::SE3Tpl< Scalar, 0 > SE3
Definition: contact-patch.hpp:22
const SE3 & placement() const
Definition: contact-patch.hpp:45
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
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
ContactPatch & contactPatch(const std::string &eeName)
Definition: contact-phase.hpp:394
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
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
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
ContactPhase & phaseAtTime(const double time)
phaseAtTime return a phase of the sequence such that : phase.timeInitial <= t < phase....
Definition: contact-sequence.hpp:1430
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
ContactPhase & contactPhase(const size_t id)
contactPhase return a reference to the contactPhase stored at the given Id
Definition: contact-sequence.hpp:85
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