contact-phase.hpp
Go to the documentation of this file.
1 #ifndef __multicontact_api_scenario_contact_phase_hpp__
2 #define __multicontact_api_scenario_contact_phase_hpp__
3 
4 #include <ndcurves/fwd.h>
5 #include <ndcurves/piecewise_curve.h>
6 
7 #include <boost/serialization/map.hpp>
8 #include <boost/serialization/string.hpp>
9 #include <boost/serialization/vector.hpp>
10 #include <map>
11 #include <ndcurves/serialization/curves.hpp>
12 #include <set>
13 #include <sstream>
14 #include <string>
15 #include <vector>
16 
23 
24 namespace multicontact_api {
25 namespace scenario {
26 
27 template <typename _Scalar>
29  : public serialization::Serializable<ContactPhaseTpl<_Scalar> > {
30  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
31 
32  typedef _Scalar Scalar;
33 
34  // Eigen types
35  typedef ndcurves::pointX_t pointX_t;
36  typedef ndcurves::point3_t point3_t;
37  typedef ndcurves::point6_t point6_t;
38  typedef ndcurves::t_point3_t t_point3_t;
39  typedef ndcurves::t_pointX_t t_pointX_t;
40  typedef ndcurves::transform_t transform_t;
41 
42  // Curves types
43  typedef ndcurves::curve_abc_t curve_t;
44  // typedef ndcurves::curve_abc<Scalar, Scalar, true, point3_t> curve_3_t;
45  typedef ndcurves::curve_SE3_t curve_SE3_t;
46  typedef ndcurves::curve_ptr_t curve_ptr;
47  // typedef boost::shared_ptr<curve_3_t> curve_3_ptr;
48  typedef ndcurves::curve_SE3_ptr_t curve_SE3_ptr;
49  typedef ndcurves::piecewise3_t piecewise3_t;
50  typedef ndcurves::piecewise_t piecewise_t;
51  typedef piecewise_t::t_time_t t_time_t;
52 
53  typedef std::vector<std::string> t_strings;
55  typedef typename ContactPatch::SE3 SE3;
56  typedef std::map<std::string, ContactPatch> ContactPatchMap;
59 
62  : m_c_init(point3_t::Zero()),
63  m_dc_init(point3_t::Zero()),
64  m_ddc_init(point3_t::Zero()),
65  m_L_init(point3_t::Zero()),
66  m_dL_init(point3_t::Zero()),
67  m_q_init(),
68  m_c_final(point3_t::Zero()),
69  m_dc_final(point3_t::Zero()),
70  m_ddc_final(point3_t::Zero()),
71  m_L_final(point3_t::Zero()),
72  m_dL_final(point3_t::Zero()),
73  m_q_final(),
74  m_q(),
75  m_dq(),
76  m_ddq(),
77  m_tau(),
78  m_c(),
79  m_dc(),
80  m_ddc(),
81  m_L(),
82  m_dL(),
83  m_wrench(),
84  m_zmp(),
85  m_root(),
91  m_t_init(-1),
92  m_t_final(-1) {}
93 
100  ContactPhaseTpl(const Scalar t_init, const Scalar t_final)
101  : m_c_init(point3_t::Zero()),
102  m_dc_init(point3_t::Zero()),
103  m_ddc_init(point3_t::Zero()),
104  m_L_init(point3_t::Zero()),
105  m_dL_init(point3_t::Zero()),
106  m_q_init(),
107  m_c_final(point3_t::Zero()),
108  m_dc_final(point3_t::Zero()),
109  m_ddc_final(point3_t::Zero()),
110  m_L_final(point3_t::Zero()),
111  m_dL_final(point3_t::Zero()),
112  m_q_final(),
113  m_q(),
114  m_dq(),
115  m_ddq(),
116  m_tau(),
117  m_c(),
118  m_dc(),
119  m_ddc(),
120  m_L(),
121  m_dL(),
122  m_wrench(),
123  m_zmp(),
124  m_root(),
130  m_t_init(t_init),
131  m_t_final(t_final) {
132  if (t_final < t_init)
133  throw std::invalid_argument("t_final cannot be inferior to t_initial");
134  }
135 
137  template <typename S2>
139  : m_c_init(other.m_c_init),
140  m_dc_init(other.m_dc_init),
141  m_ddc_init(other.m_ddc_init),
142  m_L_init(other.m_L_init),
143  m_dL_init(other.m_dL_init),
144  m_q_init(other.m_q_init),
145  m_c_final(other.m_c_final),
146  m_dc_final(other.m_dc_final),
147  m_ddc_final(other.m_ddc_final),
148  m_L_final(other.m_L_final),
149  m_dL_final(other.m_dL_final),
150  m_q_final(other.m_q_final),
151  m_q(other.m_q),
152  m_dq(other.m_dq),
153  m_ddq(other.m_ddq),
154  m_tau(other.m_tau),
155  m_c(other.m_c),
156  m_dc(other.m_dc),
157  m_ddc(other.m_ddc),
158  m_L(other.m_L),
159  m_dL(other.m_dL),
160  m_wrench(other.m_wrench),
161  m_zmp(other.m_zmp),
162  m_root(other.m_root),
168  m_t_init(other.m_t_init),
169  m_t_final(other.m_t_final) {}
170 
171  template <typename S2>
172  bool operator==(const ContactPhaseTpl<S2>& other) const {
173  return m_c_init == other.m_c_init && m_dc_init == other.m_dc_init &&
174  m_ddc_init == other.m_ddc_init && m_L_init == other.m_L_init &&
175  m_dL_init == other.m_dL_init &&
176  (m_q_init.rows() == other.m_q_init.rows() &&
177  m_q_init.cols() == other.m_q_init.cols() &&
178  m_q_init == other.m_q_init) &&
179  m_c_final == other.m_c_final && m_dc_final == other.m_dc_final &&
180  m_ddc_final == other.m_ddc_final && m_L_final == other.m_L_final &&
181  m_dL_final == other.m_dL_final &&
182  (m_q_final.rows() == other.m_q_final.rows() &&
183  m_q_final.cols() == other.m_q_final.cols() &&
184  m_q_final == other.m_q_final) &&
185  (m_q == other.m_q ||
186  (m_q && other.m_q && m_q->isApprox(other.m_q.get()))) &&
187  (m_dq == other.m_dq ||
188  (m_dq && other.m_dq && m_dq->isApprox(other.m_dq.get()))) &&
189  (m_ddq == other.m_ddq ||
190  (m_ddq && other.m_ddq && m_ddq->isApprox(other.m_ddq.get()))) &&
191  (m_tau == other.m_tau ||
192  (m_tau && other.m_tau && m_tau->isApprox(other.m_tau.get()))) &&
193  (m_c == other.m_c ||
194  (m_c && other.m_c && m_c->isApprox(other.m_c.get()))) &&
195  (m_dc == other.m_dc ||
196  (m_dc && other.m_dc && m_dc->isApprox(other.m_dc.get()))) &&
197  (m_ddc == other.m_ddc ||
198  (m_ddc && other.m_ddc && m_ddc->isApprox(other.m_ddc.get()))) &&
199  (m_L == other.m_L ||
200  (m_L && other.m_L && m_L->isApprox(other.m_L.get()))) &&
201  (m_dL == other.m_dL ||
202  (m_dL && other.m_dL && m_dL->isApprox(other.m_dL.get()))) &&
203  (m_wrench == other.m_wrench ||
204  (m_wrench && other.m_wrench &&
205  m_wrench->isApprox(other.m_wrench.get()))) &&
206  (m_zmp == other.m_zmp ||
207  (m_zmp && other.m_zmp && m_zmp->isApprox(other.m_zmp.get()))) &&
208  (m_root == other.m_root ||
209  (m_root && other.m_root && m_root->isApprox(other.m_root.get()))) &&
215  m_t_init == other.m_t_init && m_t_final == other.m_t_final;
216  }
217 
218  template <typename S2>
219  bool operator!=(const ContactPhaseTpl<S2>& other) const {
220  return !(*this == other);
221  }
222 
223  // public members :
248 
273 
274  // getter and setter for the timings
275  Scalar timeInitial() const { return m_t_init; }
276  void timeInitial(const Scalar t) { m_t_init = t; }
277  Scalar timeFinal() const { return m_t_final; }
278  void timeFinal(const Scalar t) {
279  if (t < m_t_init)
280  throw std::invalid_argument("t_final cannot be inferior to t_initial");
281  m_t_final = t;
282  }
283  Scalar duration() const { return m_t_final - m_t_init; }
284  void duration(const Scalar d) {
285  if (d <= 0)
286  throw std::invalid_argument("Duration of the phase cannot be negative.");
287  m_t_final = m_t_init + d;
288  }
289 
290  // getter for the map trajectories
294  curve_ptr contactForces(const std::string& eeName) {
295  if (m_contact_forces.count(eeName) == 0) {
296  throw std::invalid_argument(
297  "This contact phase do not contain any contact force trajectory for "
298  "the effector " +
299  eeName);
300  } else {
301  return m_contact_forces.at(eeName);
302  }
303  }
304  curve_ptr contactNormalForces(const std::string& eeName) {
305  if (m_contact_normal_force.count(eeName) == 0) {
306  throw std::invalid_argument(
307  "This contact phase do not contain any normal contact force "
308  "trajectory for the effector " +
309  eeName);
310  } else {
311  return m_contact_normal_force.at(eeName);
312  }
313  }
314  curve_SE3_ptr effectorTrajectories(const std::string& eeName) {
315  if (m_effector_trajectories.count(eeName) == 0) {
316  throw std::invalid_argument(
317  "This contact phase do not contain any effector trajectory for the "
318  "effector " +
319  eeName);
320  } else {
321  return m_effector_trajectories.at(eeName);
322  }
323  }
333  bool addContactForceTrajectory(const std::string& eeName,
334  const curve_ptr trajectory) {
335  if (!isEffectorInContact(eeName))
336  throw std::invalid_argument(
337  "Cannot add a contact force trajectory for effector " + eeName +
338  " as it is not in contact for the current phase.");
339  bool alreadyExist(m_contact_forces.count(eeName));
340  if (alreadyExist) m_contact_forces.erase(eeName);
341  m_contact_forces.insert(
342  std::pair<std::string, curve_ptr>(eeName, trajectory));
343  return !alreadyExist;
344  }
356  bool addContactNormalForceTrajectory(const std::string& eeName,
357  const curve_ptr trajectory) {
358  if (!isEffectorInContact(eeName))
359  throw std::invalid_argument(
360  "Cannot add a contact normal trajectory for effector " + eeName +
361  " as it is not in contact for the current phase.");
362  if (trajectory->dim() != 1)
363  throw std::invalid_argument(
364  "Contact normal force trajectory must be of dimension 1");
365  bool alreadyExist(m_contact_normal_force.count(eeName));
366  if (alreadyExist) m_contact_normal_force.erase(eeName);
367  m_contact_normal_force.insert(
368  std::pair<std::string, curve_ptr>(eeName, trajectory));
369  return !alreadyExist;
370  }
380  bool addEffectorTrajectory(const std::string& eeName,
381  const curve_SE3_ptr trajectory) {
382  if (isEffectorInContact(eeName))
383  throw std::invalid_argument(
384  "Cannot add an effector trajectory for effector " + eeName +
385  " as it is in contact for the current phase.");
386  bool alreadyExist(m_effector_trajectories.count(eeName));
387  if (alreadyExist) m_effector_trajectories.erase(eeName);
389  std::pair<std::string, curve_SE3_ptr>(eeName, trajectory));
390  return !alreadyExist;
391  }
392 
394  ContactPatch& contactPatch(const std::string& eeName) {
395  if (m_contact_patches.count(eeName) == 0) {
396  throw std::invalid_argument(
397  "This contact phase do not contain any contact patch for the "
398  "effector " +
399  eeName);
400  } else {
401  return m_contact_patches.at(eeName);
402  }
403  }
413  bool addContact(const std::string& eeName, const ContactPatch& patch) {
414  bool alreadyExist(isEffectorInContact(eeName));
415  if (m_contact_patches.count(eeName))
416  m_contact_patches.erase(eeName);
417  else
418  m_effector_in_contact.push_back(eeName);
419  m_contact_patches.insert(
420  std::pair<std::string, ContactPatch>(eeName, patch));
421  m_effector_trajectories.erase(eeName);
422  return !alreadyExist;
423  }
424 
432  bool removeContact(const std::string& eeName) {
433  bool existed(isEffectorInContact(eeName));
434  if (existed)
435  m_effector_in_contact.erase(std::find(
436  m_effector_in_contact.begin(), m_effector_in_contact.end(), eeName));
437  m_contact_patches.erase(eeName);
438  m_contact_forces.erase(eeName);
439  m_contact_normal_force.erase(eeName);
440  return existed;
441  }
442 
443  std::size_t numContacts() const { return m_effector_in_contact.size(); }
444 
446 
447  bool isEffectorInContact(const std::string& eeName) const {
448  if (m_effector_in_contact.empty())
449  return false;
450  else
451  return (std::find(m_effector_in_contact.begin(),
452  m_effector_in_contact.end(),
453  eeName) != m_effector_in_contact.end());
454  }
455 
463  t_strings effectors;
464  for (typename CurveSE3Map_t::const_iterator mit =
465  m_effector_trajectories.begin();
466  mit != m_effector_trajectories.end(); ++mit) {
467  effectors.push_back(mit->first);
468  }
469  return effectors;
470  }
471 
478  bool effectorHaveAtrajectory(const std::string& eeName) const {
479  return m_effector_trajectories.count(eeName);
480  }
481 
482  /* Helpers */
483 
497  bool isConsistent(const bool throw_if_inconsistent = false) const {
498  std::cout << "WARNING : not implemented yet, return True" << std::endl;
499  (void)throw_if_inconsistent;
500  return true;
501  }
502 
516  const t_pointX_t& points_derivative,
517  const t_pointX_t& points_second_derivative,
518  const t_time_t& time_points) {
519  /*
520  piecewise_t c_t =
521  piecewise_t::convert_discrete_points_to_polynomial<ndcurves::polynomial_t>(
522  points, points_derivative, points_second_derivative, time_points);
523  if (c_t.dim() != 3) throw std::invalid_argument("Dimension of the points
524  must be 3."); m_c = curve_ptr(new piecewise_t(c_t)); m_dc =
525  curve_ptr(c_t.compute_derivate_ptr(1)); m_ddc =
526  curve_ptr(c_t.compute_derivate_ptr(2));
527  */
528  m_c = curve_ptr(
529  new piecewise_t(piecewise_t::convert_discrete_points_to_polynomial<
530  ndcurves::polynomial_t>(points, time_points)));
531  m_dc = curve_ptr(new piecewise_t(
532  piecewise_t::convert_discrete_points_to_polynomial<
533  ndcurves::polynomial_t>(points_derivative, time_points)));
535  piecewise_t::convert_discrete_points_to_polynomial<
536  ndcurves::polynomial_t>(points_second_derivative, time_points)));
537  if (m_c->dim() != 3 || m_dc->dim() != 3 || m_ddc->dim() != 3)
538  throw std::invalid_argument("Dimension of the points must be 3.");
539 
540  m_c_init = point3_t(points.front());
541  m_c_final = point3_t(points.back());
542  m_dc_init = point3_t(points_derivative.front());
543  m_dc_final = point3_t(points_derivative.back());
544  m_ddc_init = point3_t(points_second_derivative.front());
545  m_ddc_final = point3_t(points_second_derivative.back());
546  return;
547  }
548 
561  const t_pointX_t& points_derivative,
562  const t_time_t& time_points) {
563  /*
564  piecewise_t L_t =
565  piecewise_t::convert_discrete_points_to_polynomial<ndcurves::polynomial_t>(
566  points, points_derivative, time_points);
567  if (L_t.dim() != 3) throw std::invalid_argument("Dimension of the points
568  must be 3."); m_L = curve_ptr(new piecewise_t(L_t)); m_dL =
569  curve_ptr(L_t.compute_derivate_ptr(1));
570  */
571  m_L = curve_ptr(
572  new piecewise_t(piecewise_t::convert_discrete_points_to_polynomial<
573  ndcurves::polynomial_t>(points, time_points)));
574  m_dL = curve_ptr(new piecewise_t(
575  piecewise_t::convert_discrete_points_to_polynomial<
576  ndcurves::polynomial_t>(points_derivative, time_points)));
577  if (m_L->dim() != 3 || m_dL->dim() != 3)
578  throw std::invalid_argument("Dimension of the points must be 3.");
579 
580  m_L_init = point3_t(points.front());
581  m_L_final = point3_t(points.back());
582  m_dL_init = point3_t(points_derivative.front());
583  m_dL_final = point3_t(points_derivative.back());
584  return;
585  }
586 
600  const t_pointX_t& points_derivative,
601  const t_pointX_t& points_second_derivative,
602  const t_time_t& time_points) {
603  /*
604  piecewise_t q_t =
605  piecewise_t::convert_discrete_points_to_polynomial<ndcurves::polynomial_t>(
606  points, points_derivative, points_second_derivative, time_points);
607  m_q = curve_ptr(new piecewise_t(q_t));
608  m_dq = curve_ptr(q_t.compute_derivate_ptr(1));
609  m_ddq = curve_ptr(q_t.compute_derivate_ptr(2));
610  */
611  m_q = curve_ptr(
612  new piecewise_t(piecewise_t::convert_discrete_points_to_polynomial<
613  ndcurves::polynomial_t>(points, time_points)));
614  m_dq = curve_ptr(new piecewise_t(
615  piecewise_t::convert_discrete_points_to_polynomial<
616  ndcurves::polynomial_t>(points_derivative, time_points)));
618  piecewise_t::convert_discrete_points_to_polynomial<
619  ndcurves::polynomial_t>(points_second_derivative, time_points)));
620  m_q_init = points.front();
621  m_q_final = points.back();
622  return;
623  }
624 
632  t_strings res;
633  for (std::string eeName : m_effector_in_contact) {
634  if (!to.isEffectorInContact(eeName)) res.push_back(eeName);
635  }
636  return res;
637  }
638 
646  t_strings res;
647  for (std::string eeName : to.effectorsInContact()) {
648  if (!isEffectorInContact(eeName)) res.push_back(eeName);
649  }
650  return res;
651  }
652 
660  t_strings res;
661  const ContactPatchMap& to_patch_map = to.contactPatches();
662  for (std::string eeName : m_effector_in_contact) {
663  if (to.isEffectorInContact(eeName))
664  if (m_contact_patches.at(eeName).placement() !=
665  to_patch_map.at(eeName).placement())
666  res.push_back(eeName);
667  }
668  return res;
669  }
670 
678  std::set<std::string>
679  set_res; // use intermediate set to guarantee uniqueness of element
680  for (std::string eeName : getContactsBroken(to)) {
681  set_res.insert(eeName);
682  }
683  for (std::string eeName : getContactsCreated(to)) {
684  set_res.insert(eeName);
685  }
686  for (std::string eeName : getContactsRepositioned(to)) {
687  set_res.insert(eeName);
688  }
689  return t_strings(set_res.begin(), set_res.end());
690  }
691 
692  /* End Helpers */
693 
694  void disp(std::ostream& os) const {
695  Eigen::Matrix<Scalar, 3, 5> state0(Eigen::Matrix<Scalar, 3, 5>::Zero());
696  Eigen::Matrix<Scalar, 3, 5> state1(Eigen::Matrix<Scalar, 3, 5>::Zero());
697  state0.block(0, 0, 3, 1) = m_c_init;
698  state0.block(0, 1, 3, 1) = m_dc_init;
699  state0.block(0, 2, 3, 1) = m_ddc_init;
700  state0.block(0, 3, 3, 1) = m_L_init;
701  state0.block(0, 4, 3, 1) = m_dL_init;
702  state1.block(0, 0, 3, 1) = m_c_final;
703  state1.block(0, 1, 3, 1) = m_dc_final;
704  state1.block(0, 2, 3, 1) = m_ddc_final;
705  state1.block(0, 3, 3, 1) = m_L_final;
706  state1.block(0, 4, 3, 1) = m_dL_final;
707 
708  os << "Contact phase defined for t \\in [" << m_t_init << ";" << m_t_final
709  << "]" << std::endl
710  << "Conecting (c0,dc0,ddc0,L0,dL0) = " << std::endl
711  << state0 << std::endl
712  << "to (c0,dc0,ddc0,L0,dL0) = " << std::endl
713  << state1 << std::endl;
714  os << "Effectors in contact " << m_effector_in_contact.size() << " : "
715  << std::endl;
716  for (t_strings::const_iterator ee = m_effector_in_contact.begin();
717  ee != m_effector_in_contact.end(); ++ee) {
718  os << "______________________________________________" << std::endl
719  << "Effector " << *ee << " contact patch:" << std::endl
720  << m_contact_patches.at(*ee) << std::endl
721  << "Has contact force trajectory : "
722  << bool(m_contact_forces.count(*ee)) << std::endl
723  << "Has contact normal force trajectory : "
724  << bool(m_contact_normal_force.count(*ee)) << std::endl;
725  }
726  }
727 
728  template <typename S2>
729  friend std::ostream& operator<<(std::ostream& os,
730  const ContactPhaseTpl<S2>& cp) {
731  cp.disp(os);
732  return os;
733  }
734 
735  protected:
736  // private members
752 
753  private:
754  // Serialization of the class
756 
757  template <class Archive>
758  void serialize(Archive& ar, const unsigned int version) {
759  // ar& boost::serialization::make_nvp("placement", m_placement);
760  unsigned int curve_version; // Curves API version related to the archive
761  // multicontact-api API version
762  if (version < 2) {
763  curve_version = 0;
764  } else {
765  curve_version = 1;
766  }
767  ndcurves::serialization::register_types<Archive>(ar, curve_version);
768  ar& boost::serialization::make_nvp("c_init", m_c_init);
769  ar& boost::serialization::make_nvp("dc_init", m_dc_init);
770  ar& boost::serialization::make_nvp("ddc_init", m_ddc_init);
771  ar& boost::serialization::make_nvp("L_init", m_L_init);
772  ar& boost::serialization::make_nvp("dL_init", m_dL_init);
773  ar& boost::serialization::make_nvp("q_init", m_q_init);
774  ar& boost::serialization::make_nvp("c_final", m_c_final);
775  ar& boost::serialization::make_nvp("dc_final", m_dc_final);
776  ar& boost::serialization::make_nvp("ddc_final", m_ddc_final);
777  ar& boost::serialization::make_nvp("L_final", m_L_final);
778  ar& boost::serialization::make_nvp("dL_final", m_dL_final);
779  ar& boost::serialization::make_nvp("q_final", m_q_final);
780  ar& boost::serialization::make_nvp("q", m_q);
781  ar& boost::serialization::make_nvp("dq", m_dq);
782  ar& boost::serialization::make_nvp("ddq", m_ddq);
783  ar& boost::serialization::make_nvp("tau", m_tau);
784  ar& boost::serialization::make_nvp("c", m_c);
785  ar& boost::serialization::make_nvp("dc", m_dc);
786  ar& boost::serialization::make_nvp("ddc", m_ddc);
787  ar& boost::serialization::make_nvp("L", m_L);
788  ar& boost::serialization::make_nvp("dL", m_dL);
789  ar& boost::serialization::make_nvp("wrench", m_wrench);
790  ar& boost::serialization::make_nvp("zmp", m_zmp);
791  ar& boost::serialization::make_nvp("root", m_root);
792  ar& boost::serialization::make_nvp("contact_forces", m_contact_forces);
793  ar& boost::serialization::make_nvp("contact_normal_force",
795  ar& boost::serialization::make_nvp("effector_trajectories",
797  ar& boost::serialization::make_nvp("effector_in_contact",
799  ar& boost::serialization::make_nvp("contact_patches", m_contact_patches);
800  ar& boost::serialization::make_nvp("t_init", m_t_init);
801  ar& boost::serialization::make_nvp("t_final", m_t_final);
802  }
803 
804 }; // struct contact phase
805 
806 } // namespace scenario
807 } // namespace multicontact_api
808 
811 
812 #endif // CONTACTPHASE_HPP
void serialize(Archive &ar, pinocchio::container::aligned_vector< T > &v, const unsigned int version)
Definition: aligned-vector.hpp:32
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
Definition: contact-phase.hpp:29
point3_t m_L_final
Final angular momentum for this contact phase.
Definition: contact-phase.hpp:243
bool operator==(const ContactPhaseTpl< S2 > &other) const
Definition: contact-phase.hpp:172
bool operator!=(const ContactPhaseTpl< S2 > &other) const
Definition: contact-phase.hpp:219
ContactPatchMap m_contact_patches
map effector name : contact patches. All the patches are actives
Definition: contact-phase.hpp:747
Scalar timeFinal() const
Definition: contact-phase.hpp:277
curve_ptr m_dq
trajectory for the joint velocities
Definition: contact-phase.hpp:252
ndcurves::transform_t transform_t
Definition: contact-phase.hpp:40
ndcurves::curve_ptr_t curve_ptr
Definition: contact-phase.hpp:46
void timeInitial(const Scalar t)
Definition: contact-phase.hpp:276
ndcurves::piecewise_t piecewise_t
Definition: contact-phase.hpp:50
curve_ptr contactNormalForces(const std::string &eeName)
Definition: contact-phase.hpp:304
void setCOMtrajectoryFromPoints(const t_pointX_t &points, const t_pointX_t &points_derivative, const t_pointX_t &points_second_derivative, const t_time_t &time_points)
setCOMtrajectoryFromPoints set the c,dc and ddc curves from a list of discrete COM positions,...
Definition: contact-phase.hpp:515
ContactPhaseTpl(const ContactPhaseTpl< S2 > &other)
Copy constructor.
Definition: contact-phase.hpp:138
ContactPatchMap contactPatches() const
Definition: contact-phase.hpp:393
ndcurves::curve_abc_t curve_t
Definition: contact-phase.hpp:43
ndcurves::t_point3_t t_point3_t
Definition: contact-phase.hpp:38
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
CurveMap_t contactForces() const
Definition: contact-phase.hpp:291
curve_ptr m_ddc
trajectory for the center of mass acceleration
Definition: contact-phase.hpp:262
ContactPhaseTpl()
Default constructor.
Definition: contact-phase.hpp:61
ContactPhaseTpl(const Scalar t_init, const Scalar t_final)
ContactPhaseTpl Constructor with time interval.
Definition: contact-phase.hpp:100
point3_t m_dL_final
Final angular momentum derivative for this contact phase.
Definition: contact-phase.hpp:245
ndcurves::curve_SE3_t curve_SE3_t
Definition: contact-phase.hpp:45
ndcurves::t_pointX_t t_pointX_t
Definition: contact-phase.hpp:39
point3_t m_ddc_final
Final acceleration of the center of mass for this contact phase.
Definition: contact-phase.hpp:241
t_strings getContactsVariations(const ContactPhase &to) const
getContactsVariations return the list of all the effectors whose contacts differ between *this and to
Definition: contact-phase.hpp:677
bool addEffectorTrajectory(const std::string &eeName, const curve_SE3_ptr trajectory)
adEffectorTrajectory add a trajectory to the map of contact forces. If a trajectory already exist for...
Definition: contact-phase.hpp:380
CurveMap_t contactNormalForces() const
Definition: contact-phase.hpp:292
void setAMtrajectoryFromPoints(const t_pointX_t &points, const t_pointX_t &points_derivative, const t_time_t &time_points)
setAMtrajectoryFromPoints set the L and d_L curves from a list of discrete Angular velocity values an...
Definition: contact-phase.hpp:560
t_strings getContactsRepositioned(const ContactPhase &to) const
getContactsRepositioned return the list of effectors in contact both in 'to' and '*this' but not at t...
Definition: contact-phase.hpp:659
curve_ptr contactForces(const std::string &eeName)
Definition: contact-phase.hpp:294
curve_SE3_ptr m_root
SE3 trajectory of the root of the robot.
Definition: contact-phase.hpp:272
void disp(std::ostream &os) const
Definition: contact-phase.hpp:694
bool isEffectorInContact(const std::string &eeName) const
Definition: contact-phase.hpp:447
t_strings getContactsCreated(const ContactPhase &to) const
getContactsCreated return the list of effectors in contact in 'to' but not in contact in '*this'
Definition: contact-phase.hpp:645
void duration(const Scalar d)
Definition: contact-phase.hpp:284
ContactPatchTpl< Scalar > ContactPatch
Definition: contact-phase.hpp:54
ndcurves::piecewise3_t piecewise3_t
Definition: contact-phase.hpp:49
point3_t m_dL_init
Initial angular momentum derivative for this contact phase.
Definition: contact-phase.hpp:233
curve_ptr m_ddq
trajectory for the joint accelerations
Definition: contact-phase.hpp:254
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
CurveMap< curve_ptr > CurveMap_t
Definition: contact-phase.hpp:57
void timeFinal(const Scalar t)
Definition: contact-phase.hpp:278
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
bool addContactForceTrajectory(const std::string &eeName, const curve_ptr trajectory)
addContactForceTrajectory add a trajectory to the map of contact forces. If a trajectory already exis...
Definition: contact-phase.hpp:333
ndcurves::point3_t point3_t
Definition: contact-phase.hpp:36
CurveMap_t m_contact_normal_force
map with keys : effector name containing the contact normal force
Definition: contact-phase.hpp:740
Scalar m_t_final
time at the end of the contact phase
Definition: contact-phase.hpp:751
Scalar m_t_init
time at the beginning of the contact phase
Definition: contact-phase.hpp:749
std::map< std::string, ContactPatch > ContactPatchMap
Definition: contact-phase.hpp:56
point3_t m_c_init
Initial position of the center of mass for this contact phase.
Definition: contact-phase.hpp:225
curve_ptr m_zmp
trajectory for the zmp
Definition: contact-phase.hpp:270
bool removeContact(const std::string &eeName)
removeContact remove the given contact This will also remove the contact_patch all the contact_forces...
Definition: contact-phase.hpp:432
point3_t m_dc_final
Final velocity of the center of mass for this contact phase.
Definition: contact-phase.hpp:239
bool isConsistent(const bool throw_if_inconsistent=false) const
isConsistent check if all the members of the phase are consistent together:
Definition: contact-phase.hpp:497
void setJointsTrajectoryFromPoints(const t_pointX_t &points, const t_pointX_t &points_derivative, const t_pointX_t &points_second_derivative, const t_time_t &time_points)
setJointsTrajectoryFromPoints set the q,dq and ddq curves from a list of discrete joints positions,...
Definition: contact-phase.hpp:599
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef _Scalar Scalar
Definition: contact-phase.hpp:32
curve_ptr m_dL
trajectory for the angular momentum derivative
Definition: contact-phase.hpp:266
curve_ptr m_q
trajectory for the joint positions
Definition: contact-phase.hpp:250
curve_SE3_ptr effectorTrajectories(const std::string &eeName)
Definition: contact-phase.hpp:314
curve_ptr m_tau
trajectory for the joint torques
Definition: contact-phase.hpp:256
curve_ptr m_L
trajectory for the angular momentum
Definition: contact-phase.hpp:264
t_strings effectorsWithTrajectory() const
effectorsWithTrajectory return a set of all effectors for which an effector trajectory have been defi...
Definition: contact-phase.hpp:462
friend std::ostream & operator<<(std::ostream &os, const ContactPhaseTpl< S2 > &cp)
Definition: contact-phase.hpp:729
bool addContactNormalForceTrajectory(const std::string &eeName, const curve_ptr trajectory)
addContactNormalForceTrajectory add a trajectory to the map of contact normal forces....
Definition: contact-phase.hpp:356
t_strings getContactsBroken(const ContactPhase &to) const
getContactsBroken return the list of effectors in contact in '*this' but not in contact in 'to'
Definition: contact-phase.hpp:631
curve_ptr m_wrench
trajectory for the centroidal wrench
Definition: contact-phase.hpp:268
ContactPatch & contactPatch(const std::string &eeName)
Definition: contact-phase.hpp:394
ndcurves::point6_t point6_t
Definition: contact-phase.hpp:37
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
curve_ptr m_dc
trajectory for the center of mass velocity
Definition: contact-phase.hpp:260
CurveSE3Map_t m_effector_trajectories
map with keys : effector name containing the end effector trajectory
Definition: contact-phase.hpp:743
Scalar duration() const
Definition: contact-phase.hpp:283
ndcurves::pointX_t pointX_t
Definition: contact-phase.hpp:35
std::size_t numContacts() const
Definition: contact-phase.hpp:443
friend class boost::serialization::access
Definition: contact-phase.hpp:755
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
t_strings m_effector_in_contact
set of the name of all effector in contact for this phase
Definition: contact-phase.hpp:745
CurveMap< curve_SE3_ptr > CurveSE3Map_t
Definition: contact-phase.hpp:58
CurveSE3Map_t effectorTrajectories() const
Definition: contact-phase.hpp:293
piecewise_t::t_time_t t_time_t
Definition: contact-phase.hpp:51
bool effectorHaveAtrajectory(const std::string &eeName) const
effectorHaveAtrajectory check if an end effector trajectory have been defined for a given effector
Definition: contact-phase.hpp:478
CurveMap_t m_contact_forces
map with keys : effector name containing the contact forces
Definition: contact-phase.hpp:738
curve_ptr m_c
trajectory for the center of mass position
Definition: contact-phase.hpp:258