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__
ndcurves::piecewise_t piecewise_t
Definition: contact-sequence.hpp:36
pinocchio::SE3Tpl< Scalar, 0 > SE3
Definition: contact-patch.hpp:22
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 haveJointsTrajectories() const
haveJointsTrajectories Check that a q trajectory is defined for each phases Also check that the time ...
Definition: contact-sequence.hpp:849
size_t size() const
Definition: contact-sequence.hpp:47
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
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
bool operator==(const ContactSequenceTpl &other) const
Definition: contact-sequence.hpp:49
friend class boost::serialization::access
Definition: contact-sequence.hpp:1466
bool haveCentroidalTrajectories() const
haveCentroidalTrajectories check that all centroidal trajectories are defined for each phases Also ch...
Definition: contact-sequence.hpp:742
piecewise_t concatenateZMPtrajectories() const
concatenateZMPtrajectories Return a piecewise curve which is the concatenation of the m_zmp curves fo...
Definition: contact-sequence.hpp:1206
Definition: contact-patch.hpp:16
ContactPhase::Scalar Scalar
Definition: contact-sequence.hpp:25
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 haveRootTrajectories() const
haveRootTrajectories check that a root trajectory exist for each contact phases. Also check that it s...
Definition: contact-sequence.hpp:1022
const SE3 & placement() const
Definition: contact-patch.hpp:45
point3_t m_L_init
Initial angular momentum for this contact phase.
Definition: contact-phase.hpp:231
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 haveCentroidalValues() const
haveCentroidalValues Check that the initial and final CoM position and AM values are defined for all ...
Definition: contact-sequence.hpp:440
void resize(const size_t size)
Definition: contact-sequence.hpp:57
void load(Archive &ar, pinocchio::container::aligned_vector< T > &v, const unsigned int version)
Definition: aligned-vector.hpp:24
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 haveAMtrajectories() const
haveAMtrajectories check that a L and dL trajectories are defined for each phases Also check that the...
Definition: contact-sequence.hpp:633
point3_t m_dL_init
Initial angular momentum derivative for this contact phase.
Definition: contact-phase.hpp:233
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
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
ContactPatch::SE3 SE3
Definition: contact-phase.hpp:55
bool haveTimings() const
haveTimings Check if all the time intervals are defined and consistent (ie. the time always increase ...
Definition: contact-sequence.hpp:287
point3_t m_ddc_final
Final acceleration of the center of mass for this contact phase.
Definition: contact-phase.hpp:241
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
bool operator!=(const ContactSequenceTpl &other) const
Definition: contact-sequence.hpp:53
point3_t m_dc_init
Initial velocity of the center of mass for this contact phase.
Definition: contact-phase.hpp:227
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef _Scalar Scalar
Definition: contact-phase.hpp:32
ContactSequenceTpl(const ContactSequenceTpl &other)
Copy contructor.
Definition: contact-sequence.hpp:44
pointX_t m_q_init
Initial whole body configuration of this phase.
Definition: contact-phase.hpp:235
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
ContactPhase & phaseAtTime(const double time)
phaseAtTime return a phase of the sequence such that : phase.timeInitial <= t < phase.timeFinal if t equal to the last phase timeFinal, this index is returned
Definition: contact-sequence.hpp:1430
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 haveJointsDerivativesTrajectories() const
haveJointsDerivativesTrajectories Check that a dq and ddq trajectories are defined for each phases Al...
Definition: contact-sequence.hpp:890
bool haveContactModelDefined() const
haveContactModelDefined check that all the contact patch have a contact_model defined ...
Definition: contact-sequence.hpp:1070
point3_t m_dL_final
Final angular momentum derivative for this contact phase.
Definition: contact-phase.hpp:245
ContactSequenceTpl(const size_t size=0)
Definition: contact-sequence.hpp:41
piecewise_t concatenateDDCtrajectories() const
concatenateDDCtrajectories Return a piecewise curve which is the concatenation of the m_ddc curves fo...
Definition: contact-sequence.hpp:1167
t_strings effectorsInContact() const
Definition: contact-phase.hpp:445
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
point3_t m_c_final
Final position of the center of mass for this contact phase.
Definition: contact-phase.hpp:237
Scalar timeFinal() const
Definition: contact-phase.hpp:277
Definition: ellipsoid.hpp:12
ndcurves::transform_t transform_t
Definition: contact-sequence.hpp:31
ndcurves::pointX_t pointX_t
Definition: contact-sequence.hpp:30
std::vector< std::string > t_strings
Definition: contact-phase.hpp:53
Scalar timeInitial() const
Definition: contact-phase.hpp:275
ndcurves::SE3Curve_t SE3Curve_t
Definition: contact-sequence.hpp:38
ndcurves::piecewise_SE3_t piecewise_SE3_t
Definition: contact-sequence.hpp:37
point3_t m_dc_final
Final velocity of the center of mass for this contact phase.
Definition: contact-phase.hpp:239
piecewise_t concatenateWrenchTrajectories() const
concatenateWrenchTrajectories Return a piecewise curve which is the concatenation of the m_wrench cur...
Definition: contact-sequence.hpp:1220
ndcurves::curve_SE3_ptr_t curve_SE3_ptr
Definition: contact-phase.hpp:48
ContactPhase::SE3 SE3
Definition: contact-sequence.hpp:27
piecewise_SE3_t concatenateRootTrajectories() const
concatenateRootTrajectories Return a piecewise curve which is the concatenation of the m_root curves ...
Definition: contact-sequence.hpp:1285
ContactPhase::t_strings t_strings
Definition: contact-sequence.hpp:26
point3_t m_ddc_init
Initial acceleration of the center of mass for this contact phase.
Definition: contact-phase.hpp:229
Definition: contact-sequence.hpp:21
bool haveFriction() const
haveFriction check that all the contact patch used in the sequence have a friction coefficient initia...
Definition: contact-sequence.hpp:1050
ndcurves::curve_SE3_t curve_SE3_t
Definition: contact-sequence.hpp:33
ndcurves::curve_abc_t curve_t
Definition: contact-sequence.hpp:32
t_strings getAllEffectorsInContact() const
getAllEffectorsInContact return a vector of names of all the effectors used to create contacts during...
Definition: contact-sequence.hpp:1124
Definition: contact-phase.hpp:28
boost::shared_ptr< curve_t > curve_ptr
Definition: contact-sequence.hpp:34
point3_t m_c_init
Initial position of the center of mass for this contact phase.
Definition: contact-phase.hpp:225
void save(Archive &ar, const pinocchio::container::aligned_vector< T > &v, const unsigned int version)
Definition: aligned-vector.hpp:16
bool haveConfigurationsValues() const
haveConfigurationsValues Check that the initial and final configuration are defined for all phases Al...
Definition: contact-sequence.hpp:450
ContactPhaseVector m_contact_phases
Definition: contact-sequence.hpp:1461
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
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
boost::shared_ptr< curve_SE3_t > curve_SE3_ptr
Definition: contact-sequence.hpp:35
void removePhase(const size_t id)
removePhase remove the given contactPhase from the sequence
Definition: contact-sequence.hpp:97
Scalar duration() const
Definition: contact-phase.hpp:283
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, an SE3 trajectory is defined for the effector that will be in contact. Also check that this trajectory is defined on the time-interval of the phase. Also check that the trajectory correctly end at the placement defined for the contact in the next phase. If this effector was in contact in the previous phase, it check that the trajectory start at the previous contact placement.
Definition: contact-sequence.hpp:756
std::vector< ContactPhase > ContactPhaseVector
Definition: contact-sequence.hpp:28
ndcurves::polynomial_t polynomial_t
Definition: contact-sequence.hpp:39
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef _ContactPhase ContactPhase
Definition: contact-sequence.hpp:24
bool haveTorquesTrajectories() const
haveJointsTrajectories Check that a joint torque trajectories are defined for each phases Also check ...
Definition: contact-sequence.hpp:941
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
size_t append(const ContactPhase &contactPhase)
append Add the given Phase at the end of the sequence
Definition: contact-sequence.hpp:66
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
piecewise_t concatenateDCtrajectories() const
concatenateDCtrajectories Return a piecewise curve which is the concatenation of the m_dc curves for ...
Definition: contact-sequence.hpp:1154
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
bool haveZMPtrajectories()
haveZMPtrajectories check that all the contact phases have a zmp trajectory
Definition: contact-sequence.hpp:1091
point3_t m_L_final
Final angular momentum for this contact phase.
Definition: contact-phase.hpp:243
pointX_t m_q_final
Final whole body configuration of this phase.
Definition: contact-phase.hpp:247
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
piecewise_t concatenateTauTrajectories() const
concatenateTauTrajectories Return a piecewise curve which is the concatenation of the m_tau curves fo...
Definition: contact-sequence.hpp:1272
bool haveContactForcesTrajectories() const
haveJointsTrajectories Check that a contact force trajectory exist for each active contact Also check...
Definition: contact-sequence.hpp:973
bool haveConsistentContacts() const
haveConsistentContacts check that there is always one contact change between adjacent phases in the s...
Definition: contact-sequence.hpp:317
piecewise_t concatenateDLtrajectories() const
concatenateDLtrajectories Return a piecewise curve which is the concatenation of the m_dL curves for ...
Definition: contact-sequence.hpp:1193
ContactPatch & contactPatch(const std::string &eeName)
Definition: contact-phase.hpp:394
ContactPhase & contactPhase(const size_t id)
contactPhase return a reference to the contactPhase stored at the given Id
Definition: contact-sequence.hpp:85
#define DEFINE_CLASS_TEMPLATE_VERSION(Template, Type)
Definition: archive.hpp:22
bool isEffectorInContact(const std::string &eeName) const
Definition: contact-phase.hpp:447