pinocchio  3.4.0
A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
contact-info.hpp
1 //
2 // Copyright (c) 2019-2023 INRIA CNRS
3 //
4 
5 #ifndef __pinocchio_algorithm_contact_info_hpp__
6 #define __pinocchio_algorithm_contact_info_hpp__
7 
8 #include <algorithm>
9 
10 #include "pinocchio/multibody/model.hpp"
11 #include "pinocchio/algorithm/fwd.hpp"
12 #include "pinocchio/algorithm/constraints/fwd.hpp"
13 #include "pinocchio/algorithm/constraints/constraint-model-base.hpp"
14 #include "pinocchio/algorithm/constraints/constraint-data-base.hpp"
15 
16 namespace pinocchio
17 {
19  enum ContactType
20  {
21  CONTACT_3D = 0,
22  CONTACT_6D,
23  CONTACT_UNDEFINED
24  };
25 
26  template<ContactType contact_type>
27  struct contact_dim
28  {
29  enum
30  {
31  value = 0
32  };
33  };
34 
35  template<>
36  struct contact_dim<CONTACT_3D>
37  {
38  enum
39  {
40  value = 3
41  };
42  };
43 
44  template<>
45  struct contact_dim<CONTACT_6D>
46  {
47  enum
48  {
49  value = 6
50  };
51  };
52 
53  template<typename _Scalar>
54  struct BaumgarteCorrectorParametersTpl;
55 
56  template<typename _Scalar>
57  struct traits<BaumgarteCorrectorParametersTpl<_Scalar>>
58  {
59  typedef _Scalar Scalar;
60  };
61 
62  template<typename _Scalar>
63  struct BaumgarteCorrectorParametersTpl : NumericalBase<BaumgarteCorrectorParametersTpl<_Scalar>>
64  {
65  typedef _Scalar Scalar;
66  typedef Eigen::Matrix<Scalar, -1, 1, Eigen::ColMajor, 6> Vector6Max;
67 
68  BaumgarteCorrectorParametersTpl(int size = 6)
69  : Kp(size)
70  , Kd(size)
71  {
72  Kp.setZero();
73  Kd.setZero();
74  }
75 
76  bool operator==(const BaumgarteCorrectorParametersTpl & other) const
77  {
78  return Kp == other.Kp && Kd == other.Kd;
79  }
80 
81  bool operator!=(const BaumgarteCorrectorParametersTpl & other) const
82  {
83  return !(*this == other);
84  }
85 
86  // parameters
88  Vector6Max Kp;
89 
91  Vector6Max Kd;
92 
93  template<typename NewScalar>
94  BaumgarteCorrectorParametersTpl<NewScalar> cast() const
95  {
96  typedef BaumgarteCorrectorParametersTpl<NewScalar> ReturnType;
97  ReturnType res;
98  res.Kp = Kp.template cast<NewScalar>();
99  res.Kd = Kd.template cast<NewScalar>();
100  return res;
101  }
102  };
103 
104  // TODO Remove when API is stabilized
105  PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
106  PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
107  template<typename NewScalar, typename Scalar, int Options>
108  struct CastType<NewScalar, RigidConstraintModelTpl<Scalar, Options>>
109  {
110  typedef RigidConstraintModelTpl<NewScalar, Options> type;
111  };
112 
113  template<typename _Scalar, int _Options>
114  struct traits<RigidConstraintModelTpl<_Scalar, _Options>>
115  {
116  typedef _Scalar Scalar;
117  enum
118  {
119  Options = _Options
120  };
121  typedef RigidConstraintDataTpl<Scalar, Options> ConstraintData;
122  };
123 
124  template<typename _Scalar, int _Options>
125  struct traits<RigidConstraintDataTpl<_Scalar, _Options>>
126  {
127  typedef _Scalar Scalar;
128  enum
129  {
130  Options = _Options
131  };
132  typedef RigidConstraintModelTpl<Scalar, Options> ConstraintModel;
133  };
134 
138  template<typename _Scalar, int _Options>
139  struct PINOCCHIO_UNSUPPORTED_MESSAGE("The API will change towards more flexibility")
140  RigidConstraintModelTpl : ConstraintModelBase<RigidConstraintModelTpl<_Scalar, _Options>>
141  {
142  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
143 
144  typedef _Scalar Scalar;
145  enum
146  {
147  Options = _Options
148  };
149  typedef ConstraintModelBase<RigidConstraintModelTpl<_Scalar, _Options>> Base;
150 
151  template<typename NewScalar, int NewOptions>
152  friend struct RigidConstraintModelTpl;
153 
154  using Base::base;
155  using Base::colwise_span_indexes;
156  using Base::colwise_sparsity;
157 
158  typedef RigidConstraintModelTpl ContactModel;
159  typedef RigidConstraintDataTpl<Scalar, Options> ContactData;
160  typedef RigidConstraintDataTpl<Scalar, Options> ConstraintData;
161 
162  typedef SE3Tpl<Scalar, Options> SE3;
163  typedef MotionTpl<Scalar, Options> Motion;
164  typedef ForceTpl<Scalar, Options> Force;
165  typedef BaumgarteCorrectorParametersTpl<Scalar> BaumgarteCorrectorParameters;
166  typedef typename Base::BooleanVector BooleanVector;
167  typedef typename Base::IndexVector IndexVector;
168  typedef Eigen::Matrix<Scalar, 3, 6, Options> Matrix36;
169 
171  ContactType type;
172 
174  JointIndex joint1_id;
175 
177  JointIndex joint2_id;
178 
180  SE3 joint1_placement;
181 
183  SE3 joint2_placement;
184 
186  ReferenceFrame reference_frame;
187 
189  SE3 desired_contact_placement;
190 
192  Motion desired_contact_velocity;
193 
195  Motion desired_contact_acceleration;
196 
198  BaumgarteCorrectorParameters corrector;
199 
201  BooleanVector colwise_joint1_sparsity;
202 
204  BooleanVector colwise_joint2_sparsity;
205 
207  IndexVector joint1_span_indexes;
208 
210  IndexVector joint2_span_indexes;
211 
212  IndexVector loop_span_indexes;
213 
215  int nv;
216 
218  size_t depth_joint1, depth_joint2;
219 
220  protected:
224  RigidConstraintModelTpl()
225  : nv(-1)
226  , depth_joint1(0)
227  , depth_joint2(0)
228  {
229  }
230 
231  public:
244  template<int OtherOptions, template<typename, int> class JointCollectionTpl>
245  RigidConstraintModelTpl(
246  const ContactType type,
247  const ModelTpl<Scalar, OtherOptions, JointCollectionTpl> & model,
248  const JointIndex joint1_id,
249  const SE3 & joint1_placement,
250  const JointIndex joint2_id,
251  const SE3 & joint2_placement,
252  const ReferenceFrame & reference_frame = LOCAL)
253  : Base(model)
254  , type(type)
255  , joint1_id(joint1_id)
256  , joint2_id(joint2_id)
257  , joint1_placement(joint1_placement)
258  , joint2_placement(joint2_placement)
259  , reference_frame(reference_frame)
260  , desired_contact_placement(SE3::Identity())
261  , desired_contact_velocity(Motion::Zero())
262  , desired_contact_acceleration(Motion::Zero())
263  , corrector(size())
264  , colwise_joint1_sparsity(model.nv)
265  , colwise_joint2_sparsity(model.nv)
266  , joint1_span_indexes((size_t)model.njoints)
267  , joint2_span_indexes((size_t)model.njoints)
268  , loop_span_indexes((size_t)model.nv)
269  {
270  init(model);
271  }
272 
282  template<int OtherOptions, template<typename, int> class JointCollectionTpl>
283  RigidConstraintModelTpl(
284  const ContactType type,
285  const ModelTpl<Scalar, OtherOptions, JointCollectionTpl> & model,
286  const JointIndex joint1_id,
287  const SE3 & joint1_placement,
288  const ReferenceFrame & reference_frame = LOCAL)
289  : Base(model)
290  , type(type)
291  , joint1_id(joint1_id)
292  , joint2_id(0)
293  , joint1_placement(joint1_placement)
294  , joint2_placement(SE3::Identity())
295  , reference_frame(reference_frame)
296  , desired_contact_placement(SE3::Identity())
297  , desired_contact_velocity(Motion::Zero())
298  , desired_contact_acceleration(Motion::Zero())
299  , corrector(size())
300  , colwise_joint1_sparsity(model.nv)
301  , colwise_joint2_sparsity(model.nv)
302  , joint1_span_indexes((size_t)model.njoints)
303  , joint2_span_indexes((size_t)model.njoints)
304  , loop_span_indexes((size_t)model.nv)
305  {
306  init(model);
307  }
308 
316  template<int OtherOptions, template<typename, int> class JointCollectionTpl>
317  RigidConstraintModelTpl(
318  const ContactType type,
319  const ModelTpl<Scalar, OtherOptions, JointCollectionTpl> & model,
320  const JointIndex joint1_id,
321  const JointIndex joint2_id,
322  const ReferenceFrame & reference_frame = LOCAL)
323  : Base(model)
324  , type(type)
325  , joint1_id(joint1_id)
326  , joint2_id(joint2_id)
327  , joint1_placement(SE3::Identity())
328  , joint2_placement(SE3::Identity())
329  , reference_frame(reference_frame)
330  , desired_contact_placement(SE3::Identity())
331  , desired_contact_velocity(Motion::Zero())
332  , desired_contact_acceleration(Motion::Zero())
333  , corrector(size())
334  , colwise_joint1_sparsity(model.nv)
335  , colwise_joint2_sparsity(model.nv)
336  , joint1_span_indexes((size_t)model.njoints)
337  , joint2_span_indexes((size_t)model.njoints)
338  , loop_span_indexes((size_t)model.nv)
339  {
340  init(model);
341  }
342 
352  template<int OtherOptions, template<typename, int> class JointCollectionTpl>
353  RigidConstraintModelTpl(
354  const ContactType type,
355  const ModelTpl<Scalar, OtherOptions, JointCollectionTpl> & model,
356  const JointIndex joint1_id,
357  const ReferenceFrame & reference_frame = LOCAL)
358  : Base(model)
359  , type(type)
360  , joint1_id(joint1_id)
361  , joint2_id(0) // set to be the Universe
362  , joint1_placement(SE3::Identity())
363  , joint2_placement(SE3::Identity())
364  , reference_frame(reference_frame)
365  , desired_contact_placement(SE3::Identity())
366  , desired_contact_velocity(Motion::Zero())
367  , desired_contact_acceleration(Motion::Zero())
368  , corrector(size())
369  , colwise_joint1_sparsity(model.nv)
370  , colwise_joint2_sparsity(model.nv)
371  , joint1_span_indexes((size_t)model.njoints)
372  , joint2_span_indexes((size_t)model.njoints)
373  , loop_span_indexes((size_t)model.nv)
374  {
375  init(model);
376  }
377 
381  ConstraintData createData() const
382  {
383  return ConstraintData(*this);
384  }
385 
394  template<int OtherOptions>
395  bool operator==(const RigidConstraintModelTpl<Scalar, OtherOptions> & other) const
396  {
397  return base() == other.base() && type == other.type && joint1_id == other.joint1_id
398  && joint2_id == other.joint2_id && joint1_placement == other.joint1_placement
399  && joint2_placement == other.joint2_placement
400  && reference_frame == other.reference_frame && corrector == other.corrector
401  && colwise_joint1_sparsity == other.colwise_joint1_sparsity
402  && colwise_joint2_sparsity == other.colwise_joint2_sparsity
403  && joint1_span_indexes == other.joint1_span_indexes
404  && joint2_span_indexes == other.joint2_span_indexes && nv == other.nv
405  && depth_joint1 == other.depth_joint1 && depth_joint2 == other.depth_joint2
406  && loop_span_indexes == other.loop_span_indexes;
407  }
408 
417  template<int OtherOptions>
418  bool operator!=(const RigidConstraintModelTpl<Scalar, OtherOptions> & other) const
419  {
420  return !(*this == other);
421  }
422 
425  template<template<typename, int> class JointCollectionTpl>
426  void calc(
427  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
428  const DataTpl<Scalar, Options, JointCollectionTpl> & data,
429  RigidConstraintDataTpl<Scalar, Options> & cdata) const
430  {
431  PINOCCHIO_UNUSED_VARIABLE(model);
432 
433  if (joint1_id > 0)
434  cdata.oMc1 = data.oMi[joint1_id] * joint1_placement;
435  else
436  cdata.oMc1 = joint1_placement;
437 
438  if (joint2_id > 0)
439  cdata.oMc2 = data.oMi[joint2_id] * joint2_placement;
440  else
441  cdata.oMc2 = joint2_placement;
442 
443  // Compute relative placement
444  cdata.c1Mc2 = cdata.oMc1.actInv(cdata.oMc2);
445  }
446 
450  Matrix36 getA1(const RigidConstraintDataTpl<Scalar, Options> & cdata) const
451  {
452  Matrix36 res;
453  const SE3 & oMl = cdata.oMc1;
454  typedef typename SE3::Vector3 Vector3;
455 
456 #define PINOCCHIO_INTERNAL_COMPUTATION(axis_id, v3_in, res) \
457  CartesianAxis<axis_id>::cross(v3_in, v_tmp); \
458  res.col(axis_id).noalias() = oMl.rotation().transpose() * v_tmp;
459 
460  res.template leftCols<3>() = oMl.rotation().transpose();
461  Vector3 v_tmp;
462  PINOCCHIO_INTERNAL_COMPUTATION(0, oMl.translation(), res.template rightCols<3>());
463  PINOCCHIO_INTERNAL_COMPUTATION(1, oMl.translation(), res.template rightCols<3>());
464  PINOCCHIO_INTERNAL_COMPUTATION(2, oMl.translation(), res.template rightCols<3>());
465 
466 #undef PINOCCHIO_INTERNAL_COMPUTATION
467 
468  return res;
469  }
470 
474  Matrix36 getA2(const RigidConstraintDataTpl<Scalar, Options> & cdata) const
475  {
476  Matrix36 res;
477  const SE3 & oM1 = cdata.oMc1;
478  const SE3 & oM2 = cdata.oMc2;
479  typedef typename SE3::Vector3 Vector3;
480 
481 #define PINOCCHIO_INTERNAL_COMPUTATION(axis_id, v3_in, res) \
482  CartesianAxis<axis_id>::cross(v3_in, v_tmp); \
483  res.col(axis_id).noalias() = oM1.rotation().transpose() * v_tmp;
484 
485  res.template leftCols<3>() = -oM1.rotation().transpose();
486  Vector3 v_tmp;
487  PINOCCHIO_INTERNAL_COMPUTATION(0, -oM2.translation(), res.template rightCols<3>());
488  PINOCCHIO_INTERNAL_COMPUTATION(1, -oM2.translation(), res.template rightCols<3>());
489  PINOCCHIO_INTERNAL_COMPUTATION(2, -oM2.translation(), res.template rightCols<3>());
490 
491 #undef PINOCCHIO_INTERNAL_COMPUTATION
492 
493  return res;
494  }
495 
496  template<
497  typename InputMatrix,
498  typename OutputMatrix,
499  template<typename, int> class JointCollectionTpl>
500  void jacobian_matrix_product(
501  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
502  const DataTpl<Scalar, Options, JointCollectionTpl> & data,
503  RigidConstraintDataTpl<Scalar, Options> & cdata,
504  const Eigen::MatrixBase<InputMatrix> & mat,
505  const Eigen::MatrixBase<OutputMatrix> & _res) const
506  {
507  typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
508  typedef typename Data::Vector3 Vector3;
509  OutputMatrix & res = _res.const_cast_derived();
510 
511  PINOCCHIO_CHECK_ARGUMENT_SIZE(mat.rows(), model.nv);
512  PINOCCHIO_CHECK_ARGUMENT_SIZE(mat.cols(), res.cols());
513  PINOCCHIO_CHECK_ARGUMENT_SIZE(res.rows(), size());
514  res.setZero();
515 
516  // const Eigen::DenseIndex constraint_dim = size();
517  //
518  // const Eigen::DenseIndex
519  // complexity_strategy_1 = 6 * res.cols() * 36 + constraint_dim * 36 * res.cols(),
520  // complexity_strategy_2 = 36 * constraint_dim * 6 + constraint_dim * 36 * res.cols();
521 
522  const Matrix36 A1 = getA1(cdata);
523  const Matrix36 A2 = getA2(cdata);
524  for (Eigen::DenseIndex jj = 0; jj < model.nv; ++jj)
525  {
526  if (!(colwise_joint1_sparsity[jj] || colwise_joint2_sparsity[jj]))
527  continue;
528  Matrix36 A;
529  Vector3 AxSi;
530 
531  typedef typename Data::Matrix6x::ConstColXpr ConstColXpr;
532  const ConstColXpr Jcol = data.J.col(jj);
533 
534  if (colwise_joint1_sparsity[jj] && colwise_joint2_sparsity[jj])
535  {
536  A = A1 + A2;
537  AxSi.noalias() = A * Jcol;
538  }
539  else if (colwise_joint1_sparsity[jj])
540  AxSi.noalias() = A1 * Jcol;
541  else
542  AxSi.noalias() = A2 * Jcol;
543 
544  res.noalias() += AxSi * mat.row(jj);
545  }
546  }
547 
550  template<typename JacobianMatrix, template<typename, int> class JointCollectionTpl>
551  void jacobian(
552  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
553  const DataTpl<Scalar, Options, JointCollectionTpl> & data,
554  RigidConstraintDataTpl<Scalar, Options> & cdata,
555  const Eigen::MatrixBase<JacobianMatrix> & _jacobian_matrix) const
556  {
557  typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
558  JacobianMatrix & jacobian_matrix = _jacobian_matrix.const_cast_derived();
559 
560  const RigidConstraintModelTpl & cmodel = *this;
561 
562  const SE3 & oMc1 = cdata.oMc1;
563  const SE3 & oMc2 = cdata.oMc2;
564  const SE3 & c1Mc2 = cdata.c1Mc2;
565 
566  for (Eigen::DenseIndex jj = 0; jj < model.nv; ++jj)
567  {
568 
569  if (colwise_joint1_sparsity[jj] || colwise_joint2_sparsity[jj])
570  {
571  const int sign = colwise_joint1_sparsity[jj] != colwise_joint2_sparsity[jj]
572  ? colwise_joint1_sparsity[jj] ? +1 : -1
573  : 0; // specific case for CONTACT_3D
574 
575  typedef typename Data::Matrix6x::ConstColXpr ConstColXpr;
576  const ConstColXpr Jcol = data.J.col(jj);
577  const MotionRef<const ConstColXpr> Jcol_motion(Jcol);
578 
579  switch (cmodel.type)
580  {
581  case CONTACT_3D: {
582  switch (cmodel.reference_frame)
583  {
584  case LOCAL: {
585  if (sign == 0)
586  {
587  const Motion Jcol_local1(oMc1.actInv(Jcol_motion)); // TODO: simplify computations
588  const Motion Jcol_local2(oMc2.actInv(Jcol_motion)); // TODO: simplify computations
589  const typename Motion::Vector3 Jdiff_linear =
590  Jcol_local1.linear() - c1Mc2.rotation() * Jcol_local2.linear();
591  jacobian_matrix.col(jj) = Jdiff_linear;
592  break;
593  }
594  else if (sign == 1)
595  {
596  const Motion Jcol_local(oMc1.actInv(Jcol_motion));
597  jacobian_matrix.col(jj) = Jcol_local.linear();
598  break;
599  }
600  else // sign == -1
601  {
602  Motion Jcol_local(oMc2.actInv(Jcol_motion)); // TODO: simplify computations
603  Jcol_local.linear() =
604  c1Mc2.rotation() * Jcol_local.linear(); // TODO: simplify computations
605  jacobian_matrix.col(jj) = -Jcol_local.linear();
606  break;
607  }
608  }
609  case LOCAL_WORLD_ALIGNED: {
610  if (sign == 0)
611  {
612  const typename Motion::Vector3 Jdiff_linear =
613  (oMc2.translation() - oMc1.translation()).cross(Jcol_motion.angular());
614  jacobian_matrix.col(jj) = Jdiff_linear;
615  break;
616  }
617  else
618  {
619  typename Motion::Vector3 Jcol_local_world_aligned_linear(Jcol_motion.linear());
620  if (sign == 1)
621  Jcol_local_world_aligned_linear -=
622  oMc1.translation().cross(Jcol_motion.angular());
623  else
624  Jcol_local_world_aligned_linear -=
625  oMc2.translation().cross(Jcol_motion.angular());
626  jacobian_matrix.col(jj) = Jcol_local_world_aligned_linear * Scalar(sign);
627  break;
628  }
629  }
630  case WORLD: {
631  PINOCCHIO_THROW_PRETTY(
632  std::invalid_argument, "Contact3D in world frame is not managed");
633  }
634  }
635  break;
636  }
637 
638  case CONTACT_6D: {
639  assert(
640  check_expression_if_real<Scalar>(sign != 0) && "sign should be equal to +1 or -1.");
641  switch (cmodel.reference_frame)
642  {
643  case LOCAL: {
644  const Motion Jcol_local(oMc1.actInv(Jcol_motion));
645  jacobian_matrix.col(jj) = Jcol_local.toVector() * Scalar(sign);
646  break;
647  }
648  case LOCAL_WORLD_ALIGNED: {
649  Motion Jcol_local_world_aligned(Jcol_motion);
650  Jcol_local_world_aligned.linear() -=
651  oMc1.translation().cross(Jcol_local_world_aligned.angular());
652  jacobian_matrix.col(jj) = Jcol_local_world_aligned.toVector() * Scalar(sign);
653  break;
654  }
655  case WORLD: {
656  PINOCCHIO_THROW_PRETTY(
657  std::invalid_argument, "Contact6D in world frame is not managed");
658  }
659  }
660  break;
661  }
662 
663  default:
664  assert(false && "must never happened");
665  break;
666  }
667  }
668  }
669  }
670 
671  int size() const
672  {
673  switch (type)
674  {
675  case CONTACT_3D:
676  return contact_dim<CONTACT_3D>::value;
677  case CONTACT_6D:
678  return contact_dim<CONTACT_6D>::value;
679  default:
680  return contact_dim<CONTACT_UNDEFINED>::value;
681  }
682  return -1;
683  }
684 
686  template<typename NewScalar>
687  RigidConstraintModelTpl<NewScalar, Options> cast() const
688  {
689  typedef RigidConstraintModelTpl<NewScalar, Options> ReturnType;
690  ReturnType res;
691  res.base() = base();
692  res.type = type;
693  res.joint1_id = joint1_id;
694  res.joint2_id = joint2_id;
695  res.joint1_placement = joint1_placement.template cast<NewScalar>();
696  res.joint2_placement = joint2_placement.template cast<NewScalar>();
697  res.reference_frame = reference_frame;
698  res.desired_contact_placement = desired_contact_placement.template cast<NewScalar>();
699  res.desired_contact_velocity = desired_contact_velocity.template cast<NewScalar>();
700  res.desired_contact_acceleration = desired_contact_acceleration.template cast<NewScalar>();
701  res.corrector = corrector.template cast<NewScalar>();
702  res.colwise_joint1_sparsity = colwise_joint1_sparsity;
703  res.colwise_joint2_sparsity = colwise_joint2_sparsity;
704  res.nv = nv;
705  res.depth_joint1 = depth_joint1;
706  res.depth_joint2 = depth_joint2;
707  res.loop_span_indexes = loop_span_indexes;
708 
709  return res;
710  }
711 
712  protected:
713  template<int OtherOptions, template<typename, int> class JointCollectionTpl>
714  void init(const ModelTpl<Scalar, OtherOptions, JointCollectionTpl> & model)
715  {
716  PINOCCHIO_CHECK_INPUT_ARGUMENT(
717  reference_frame == LOCAL || reference_frame == LOCAL_WORLD_ALIGNED,
718  "reference_frame should be LOCAL or LOCAL_WORLD_ALIGNED");
719  nv = model.nv;
720  depth_joint1 = static_cast<size_t>(model.supports[joint1_id].size());
721  depth_joint2 = static_cast<size_t>(model.supports[joint2_id].size());
722 
723  typedef ModelTpl<Scalar, OtherOptions, JointCollectionTpl> Model;
724  typedef typename Model::JointModel JointModel;
725  static const bool default_sparsity_value = false;
726  colwise_joint1_sparsity.fill(default_sparsity_value);
727  colwise_joint2_sparsity.fill(default_sparsity_value);
728  joint1_span_indexes.reserve((size_t)model.njoints);
729  joint2_span_indexes.reserve((size_t)model.njoints);
730 
731  JointIndex current1_id = 0;
732  if (joint1_id > 0)
733  current1_id = joint1_id;
734 
735  JointIndex current2_id = 0;
736  if (joint2_id > 0)
737  current2_id = joint2_id;
738 
739  while (current1_id != current2_id)
740  {
741  if (current1_id > current2_id)
742  {
743  const JointModel & joint1 = model.joints[current1_id];
744  const int j1nv = joint1.nv();
745  joint1_span_indexes.push_back((Eigen::DenseIndex)current1_id);
746  Eigen::DenseIndex current1_col_id = joint1.idx_v();
747  for (int k = 0; k < j1nv; ++k, ++current1_col_id)
748  {
749  colwise_joint1_sparsity[current1_col_id] = true;
750  }
751  current1_id = model.parents[current1_id];
752  }
753  else
754  {
755  const JointModel & joint2 = model.joints[current2_id];
756  const int j2nv = joint2.nv();
757  joint2_span_indexes.push_back((Eigen::DenseIndex)current2_id);
758  Eigen::DenseIndex current2_col_id = joint2.idx_v();
759  for (int k = 0; k < j2nv; ++k, ++current2_col_id)
760  {
761  colwise_joint2_sparsity[current2_col_id] = true;
762  }
763  current2_id = model.parents[current2_id];
764  }
765  }
766  assert(current1_id == current2_id && "current1_id should be equal to current2_id");
767 
768  if (type == CONTACT_3D)
769  {
770  JointIndex current_id = current1_id;
771  while (current_id > 0)
772  {
773  const JointModel & joint = model.joints[current_id];
774  const int jnv = joint.nv();
775  joint1_span_indexes.push_back((Eigen::DenseIndex)current_id);
776  joint2_span_indexes.push_back((Eigen::DenseIndex)current_id);
777  Eigen::DenseIndex current_row_id = joint.idx_v();
778  for (int k = 0; k < jnv; ++k, ++current_row_id)
779  {
780  colwise_joint1_sparsity[current_row_id] = true;
781  colwise_joint2_sparsity[current_row_id] = true;
782  }
783  current_id = model.parents[current_id];
784  }
785  }
786 
787  std::rotate(
788  joint1_span_indexes.rbegin(), joint1_span_indexes.rbegin() + 1, joint1_span_indexes.rend());
789  std::rotate(
790  joint2_span_indexes.rbegin(), joint2_span_indexes.rbegin() + 1, joint2_span_indexes.rend());
791  Base::colwise_span_indexes.reserve((size_t)model.nv);
792  loop_span_indexes.reserve((size_t)model.nv);
793  for (Eigen::DenseIndex col_id = 0; col_id < model.nv; ++col_id)
794  {
795  if (colwise_joint1_sparsity[col_id] || colwise_joint2_sparsity[col_id])
796  {
797  colwise_span_indexes.push_back(col_id);
798  colwise_sparsity[col_id] = true;
799  }
800 
801  if (colwise_joint1_sparsity[col_id] != colwise_joint2_sparsity[col_id])
802  {
803  loop_span_indexes.push_back(col_id);
804  }
805  }
806  }
807  };
808 
809  template<typename Scalar, int Options, class Allocator>
810  size_t getTotalConstraintSize(
811  const std::vector<RigidConstraintModelTpl<Scalar, Options>, Allocator> & contact_models)
812  {
813  typedef std::vector<RigidConstraintModelTpl<Scalar, Options>, Allocator> VectorType;
814  size_t total_size = 0;
815  for (typename VectorType::const_iterator it = contact_models.begin();
816  it != contact_models.end(); ++it)
817  total_size += it->size();
818 
819  return total_size;
820  }
821 
825  template<typename _Scalar, int _Options>
826  struct PINOCCHIO_UNSUPPORTED_MESSAGE("The API will change towards more flexibility")
827  RigidConstraintDataTpl : ConstraintDataBase<RigidConstraintDataTpl<_Scalar, _Options>>
828  {
829  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
830 
831  typedef _Scalar Scalar;
832  enum
833  {
834  Options = _Options
835  };
836 
837  typedef RigidConstraintModelTpl<Scalar, Options> ContactModel;
838  typedef RigidConstraintDataTpl ContactData;
839 
840  typedef SE3Tpl<Scalar, Options> SE3;
841  typedef MotionTpl<Scalar, Options> Motion;
842  typedef ForceTpl<Scalar, Options> Force;
843  typedef Eigen::Matrix<Scalar, 6, 6, Options> Matrix6;
844  typedef PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(Matrix6) VectorOfMatrix6;
845  typedef Eigen::Matrix<Scalar, 6, Eigen::Dynamic, Options> Matrix6x;
846  typedef Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, Options> MatrixX;
847 
848  // data
849 
851  Force contact_force;
852 
854  SE3 oMc1;
855 
857  SE3 oMc2;
858 
860  SE3 c1Mc2;
861 
863  Motion contact_placement_error;
864 
866  Motion contact1_velocity;
867 
869  Motion contact2_velocity;
870 
872  Motion contact_velocity_error;
873 
875  Motion contact_acceleration;
876 
878  Motion contact_acceleration_desired;
879 
881  Motion contact_acceleration_error;
882 
885  Motion contact1_acceleration_drift;
886 
889  Motion contact2_acceleration_drift;
890 
892  Motion contact_acceleration_deviation;
893 
894  VectorOfMatrix6 extended_motion_propagators_joint1;
895  VectorOfMatrix6 lambdas_joint1;
896  VectorOfMatrix6 extended_motion_propagators_joint2;
897 
898  Matrix6x dv1_dq, da1_dq, da1_dv, da1_da;
899  Matrix6x dv2_dq, da2_dq, da2_dv, da2_da;
900  MatrixX dvc_dq, dac_dq, dac_dv, dac_da;
901 
903  RigidConstraintDataTpl()
904  : contact_force(Force::Zero())
905  , oMc1(SE3::Identity())
906  , oMc2(SE3::Identity())
907  , c1Mc2(SE3::Identity())
908  , contact_placement_error(Motion::Zero())
909  , contact1_velocity(Motion::Zero())
910  , contact2_velocity(Motion::Zero())
911  , contact_velocity_error(Motion::Zero())
912  , contact_acceleration(Motion::Zero())
913  , contact_acceleration_desired(Motion::Zero())
914  , contact_acceleration_error(Motion::Zero())
915  , contact1_acceleration_drift(Motion::Zero())
916  , contact2_acceleration_drift(Motion::Zero())
917  , contact_acceleration_deviation(Motion::Zero())
918  , extended_motion_propagators_joint1()
919  , lambdas_joint1()
920  , extended_motion_propagators_joint2()
921  , dv1_dq(6, 0)
922  , da1_dq(6, 0)
923  , da1_dv(6, 0)
924  , da1_da(6, 0)
925  , dv2_dq(6, 0)
926  , da2_dq(6, 0)
927  , da2_dv(6, 0)
928  , da2_da(6, 0)
929  , dvc_dq()
930  , dac_dq()
931  , dac_dv()
932  , dac_da()
933  {
934  }
935 
936  explicit RigidConstraintDataTpl(const ContactModel & contact_model)
937  : contact_force(Force::Zero())
938  , oMc1(SE3::Identity())
939  , oMc2(SE3::Identity())
940  , c1Mc2(SE3::Identity())
941  , contact_placement_error(Motion::Zero())
942  , contact1_velocity(Motion::Zero())
943  , contact2_velocity(Motion::Zero())
944  , contact_velocity_error(Motion::Zero())
945  , contact_acceleration(Motion::Zero())
946  , contact_acceleration_desired(Motion::Zero())
947  , contact_acceleration_error(Motion::Zero())
948  , contact1_acceleration_drift(Motion::Zero())
949  , contact2_acceleration_drift(Motion::Zero())
950  , contact_acceleration_deviation(Motion::Zero())
951  , extended_motion_propagators_joint1(contact_model.depth_joint1, Matrix6::Zero())
952  , lambdas_joint1(contact_model.depth_joint1, Matrix6::Zero())
953  , extended_motion_propagators_joint2(contact_model.depth_joint2, Matrix6::Zero())
954  , dv1_dq(Matrix6x::Zero(6, contact_model.nv))
955  , da1_dq(Matrix6x::Zero(6, contact_model.nv))
956  , da1_dv(Matrix6x::Zero(6, contact_model.nv))
957  , da1_da(Matrix6x::Zero(6, contact_model.nv))
958  , dv2_dq(Matrix6x::Zero(6, contact_model.nv))
959  , da2_dq(Matrix6x::Zero(6, contact_model.nv))
960  , da2_dv(Matrix6x::Zero(6, contact_model.nv))
961  , da2_da(Matrix6x::Zero(6, contact_model.nv))
962  , dvc_dq(MatrixX::Zero(contact_model.size(), contact_model.nv))
963  , dac_dq(MatrixX::Zero(contact_model.size(), contact_model.nv))
964  , dac_dv(MatrixX::Zero(contact_model.size(), contact_model.nv))
965  , dac_da(MatrixX::Zero(contact_model.size(), contact_model.nv))
966  {
967  }
968 
969  bool operator==(const RigidConstraintDataTpl & other) const
970  {
971  return contact_force == other.contact_force && oMc1 == other.oMc1 && oMc2 == other.oMc2
972  && c1Mc2 == other.c1Mc2 && contact_placement_error == other.contact_placement_error
973  && contact1_velocity == other.contact1_velocity
974  && contact2_velocity == other.contact2_velocity
975  && contact_velocity_error == other.contact_velocity_error
976  && contact_acceleration == other.contact_acceleration
977  && contact_acceleration_desired == other.contact_acceleration_desired
978  && contact_acceleration_error == other.contact_acceleration_error
979  && contact1_acceleration_drift == other.contact1_acceleration_drift
980  && contact2_acceleration_drift == other.contact2_acceleration_drift
981  && contact_acceleration_deviation == other.contact_acceleration_deviation
982  && extended_motion_propagators_joint1 == other.extended_motion_propagators_joint1
983  && lambdas_joint1 == other.lambdas_joint1
984  && extended_motion_propagators_joint2 == other.extended_motion_propagators_joint2
985  //
986  && dv1_dq == other.dv1_dq && da1_dq == other.da1_dq && da1_dv == other.da1_dv
987  && da1_da == other.da1_da
988  //
989  && dv2_dq == other.dv2_dq && da2_dq == other.da2_dq && da2_dv == other.da2_dv
990  && da2_da == other.da2_da
991  //
992  && dvc_dq == other.dvc_dq && dac_dq == other.dac_dq && dac_dv == other.dac_dv
993  && dac_da == other.dac_da;
994  }
995 
996  bool operator!=(const RigidConstraintDataTpl & other) const
997  {
998  return !(*this == other);
999  }
1000  };
1001  PINOCCHIO_COMPILER_DIAGNOSTIC_POP
1002 
1003 } // namespace pinocchio
1004 
1005 #endif // ifndef __pinocchio_algorithm_contact_info_hpp__
pinocchio::ReferenceFrame
ReferenceFrame
Various conventions to express the velocity of a moving frame.
Definition: fwd.hpp:47
pinocchio::WORLD
@ WORLD
Definition: fwd.hpp:48
pinocchio::LOCAL_WORLD_ALIGNED
@ LOCAL_WORLD_ALIGNED
Definition: fwd.hpp:52
pinocchio::LOCAL
@ LOCAL
Definition: fwd.hpp:50
pinocchio
Main pinocchio namespace.
Definition: treeview.dox:11
pinocchio::PINOCCHIO_UNSUPPORTED_MESSAGE
struct PINOCCHIO_UNSUPPORTED_MESSAGE("The API will change towards more flexibility") ContactCholeskyDecompositionTpl
Contact Cholesky decomposition structure. This structure allows to compute in a efficient and parsimo...
Definition: contact-cholesky.hpp:55
pinocchio::ContactType
ContactType
&#160;
Definition: contact-info.hpp:20
pinocchio::CONTACT_6D
@ CONTACT_6D
Point contact model.
Definition: contact-info.hpp:22
pinocchio::CONTACT_UNDEFINED
@ CONTACT_UNDEFINED
&#160;
Definition: contact-info.hpp:23
pinocchio::sign
Scalar sign(const Scalar &t)
Returns the robust sign of t.
Definition: sign.hpp:14
pinocchio::nv
int nv(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointNvVisitor to get the dimension of the joint tangent space.
pinocchio::getTotalConstraintSize
struct PINOCCHIO_UNSUPPORTED_MESSAGE("The API will change towards more flexibility") RigidConstraintModelTpl size_t getTotalConstraintSize(const std::vector< RigidConstraintModelTpl< Scalar, Options >, Allocator > &contact_models)
Contact model structure containg all the info describing the rigid contact model.
Definition: contact-info.hpp:810
pinocchio::cross
void cross(const Eigen::MatrixBase< Vector3 > &v, const Eigen::MatrixBase< Matrix3xIn > &Min, const Eigen::MatrixBase< Matrix3xOut > &Mout)
Applies the cross product onto the columns of M.
Definition: skew.hpp:228
pinocchio::BaumgarteCorrectorParametersTpl::Kp
Vector6Max Kp
Proportional corrector value.
Definition: contact-info.hpp:88
pinocchio::BaumgarteCorrectorParametersTpl::Kd
Vector6Max Kd
Damping corrector value.
Definition: contact-info.hpp:91
pinocchio::CastType
Type of the cast of a class C templated by Scalar and Options, to a new NewScalar type....
Definition: fwd.hpp:99
pinocchio::contact_dim
Definition: contact-info.hpp:28
pinocchio::traits
Common traits structure to fully define base classes for CRTP.
Definition: fwd.hpp:72