pinocchio  3.2.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>
500  class JointCollectionTpl>
501  void jacobian_matrix_product(
502  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
503  const DataTpl<Scalar, Options, JointCollectionTpl> & data,
504  RigidConstraintDataTpl<Scalar, Options> & cdata,
505  const Eigen::MatrixBase<InputMatrix> & mat,
506  const Eigen::MatrixBase<OutputMatrix> & _res) const
507  {
508  typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
509  typedef typename Data::Vector3 Vector3;
510  OutputMatrix & res = _res.const_cast_derived();
511 
512  PINOCCHIO_CHECK_ARGUMENT_SIZE(mat.rows(), model.nv);
513  PINOCCHIO_CHECK_ARGUMENT_SIZE(mat.cols(), res.cols());
514  PINOCCHIO_CHECK_ARGUMENT_SIZE(res.rows(), size());
515  res.setZero();
516 
517  // const Eigen::DenseIndex constraint_dim = size();
518  //
519  // const Eigen::DenseIndex
520  // complexity_strategy_1 = 6 * res.cols() * 36 + constraint_dim * 36 * res.cols(),
521  // complexity_strategy_2 = 36 * constraint_dim * 6 + constraint_dim * 36 * res.cols();
522 
523  const Matrix36 A1 = getA1(cdata);
524  const Matrix36 A2 = getA2(cdata);
525  for (Eigen::DenseIndex jj = 0; jj < model.nv; ++jj)
526  {
527  if (!(colwise_joint1_sparsity[jj] || colwise_joint2_sparsity[jj]))
528  continue;
529  Matrix36 A;
530  Vector3 AxSi;
531 
532  typedef typename Data::Matrix6x::ConstColXpr ConstColXpr;
533  const ConstColXpr Jcol = data.J.col(jj);
534 
535  if (colwise_joint1_sparsity[jj] && colwise_joint2_sparsity[jj])
536  {
537  A = A1 + A2;
538  AxSi.noalias() = A * Jcol;
539  }
540  else if (colwise_joint1_sparsity[jj])
541  AxSi.noalias() = A1 * Jcol;
542  else
543  AxSi.noalias() = A2 * Jcol;
544 
545  res.noalias() += AxSi * mat.row(jj);
546  }
547  }
548 
551  template<typename JacobianMatrix, template<typename, int> class JointCollectionTpl>
552  void jacobian(
553  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
554  const DataTpl<Scalar, Options, JointCollectionTpl> & data,
555  RigidConstraintDataTpl<Scalar, Options> & cdata,
556  const Eigen::MatrixBase<JacobianMatrix> & _jacobian_matrix) const
557  {
558  typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
559  JacobianMatrix & jacobian_matrix = _jacobian_matrix.const_cast_derived();
560 
561  const RigidConstraintModelTpl & cmodel = *this;
562 
563  const SE3 & oMc1 = cdata.oMc1;
564  const SE3 & oMc2 = cdata.oMc2;
565  const SE3 & c1Mc2 = cdata.c1Mc2;
566 
567  for (Eigen::DenseIndex jj = 0; jj < model.nv; ++jj)
568  {
569 
570  if (colwise_joint1_sparsity[jj] || colwise_joint2_sparsity[jj])
571  {
572  const int sign = colwise_joint1_sparsity[jj] != colwise_joint2_sparsity[jj]
573  ? colwise_joint1_sparsity[jj] ? +1 : -1
574  : 0; // specific case for CONTACT_3D
575 
576  typedef typename Data::Matrix6x::ConstColXpr ConstColXpr;
577  const ConstColXpr Jcol = data.J.col(jj);
578  const MotionRef<const ConstColXpr> Jcol_motion(Jcol);
579 
580  switch (cmodel.type)
581  {
582  case CONTACT_3D: {
583  switch (cmodel.reference_frame)
584  {
585  case LOCAL: {
586  if (sign == 0)
587  {
588  const Motion Jcol_local1(oMc1.actInv(Jcol_motion)); // TODO: simplify computations
589  const Motion Jcol_local2(oMc2.actInv(Jcol_motion)); // TODO: simplify computations
590  const typename Motion::Vector3 Jdiff_linear =
591  Jcol_local1.linear() - c1Mc2.rotation() * Jcol_local2.linear();
592  jacobian_matrix.col(jj) = Jdiff_linear;
593  break;
594  }
595  else if (sign == 1)
596  {
597  const Motion Jcol_local(oMc1.actInv(Jcol_motion));
598  jacobian_matrix.col(jj) = Jcol_local.linear();
599  break;
600  }
601  else // sign == -1
602  {
603  Motion Jcol_local(oMc2.actInv(Jcol_motion)); // TODO: simplify computations
604  Jcol_local.linear() =
605  c1Mc2.rotation() * Jcol_local.linear(); // TODO: simplify computations
606  jacobian_matrix.col(jj) = -Jcol_local.linear();
607  break;
608  }
609  }
610  case LOCAL_WORLD_ALIGNED: {
611  if (sign == 0)
612  {
613  const typename Motion::Vector3 Jdiff_linear =
614  (oMc2.translation() - oMc1.translation()).cross(Jcol_motion.angular());
615  jacobian_matrix.col(jj) = Jdiff_linear;
616  break;
617  }
618  else
619  {
620  typename Motion::Vector3 Jcol_local_world_aligned_linear(Jcol_motion.linear());
621  if (sign == 1)
622  Jcol_local_world_aligned_linear -=
623  oMc1.translation().cross(Jcol_motion.angular());
624  else
625  Jcol_local_world_aligned_linear -=
626  oMc2.translation().cross(Jcol_motion.angular());
627  jacobian_matrix.col(jj) = Jcol_local_world_aligned_linear * Scalar(sign);
628  break;
629  }
630  }
631  case WORLD: {
632  PINOCCHIO_THROW_PRETTY(
633  std::invalid_argument, "Contact3D in world frame is not managed");
634  }
635  }
636  break;
637  }
638 
639  case CONTACT_6D: {
640  assert(
641  check_expression_if_real<Scalar>(sign != 0) && "sign should be equal to +1 or -1.");
642  switch (cmodel.reference_frame)
643  {
644  case LOCAL: {
645  const Motion Jcol_local(oMc1.actInv(Jcol_motion));
646  jacobian_matrix.col(jj) = Jcol_local.toVector() * Scalar(sign);
647  break;
648  }
649  case LOCAL_WORLD_ALIGNED: {
650  Motion Jcol_local_world_aligned(Jcol_motion);
651  Jcol_local_world_aligned.linear() -=
652  oMc1.translation().cross(Jcol_local_world_aligned.angular());
653  jacobian_matrix.col(jj) = Jcol_local_world_aligned.toVector() * Scalar(sign);
654  break;
655  }
656  case WORLD: {
657  PINOCCHIO_THROW_PRETTY(
658  std::invalid_argument, "Contact6D in world frame is not managed");
659  }
660  }
661  break;
662  }
663 
664  default:
665  assert(false && "must never happened");
666  break;
667  }
668  }
669  }
670  }
671 
672  int size() const
673  {
674  switch (type)
675  {
676  case CONTACT_3D:
677  return contact_dim<CONTACT_3D>::value;
678  case CONTACT_6D:
679  return contact_dim<CONTACT_6D>::value;
680  default:
681  return contact_dim<CONTACT_UNDEFINED>::value;
682  }
683  return -1;
684  }
685 
687  template<typename NewScalar>
688  RigidConstraintModelTpl<NewScalar, Options> cast() const
689  {
690  typedef RigidConstraintModelTpl<NewScalar, Options> ReturnType;
691  ReturnType res;
692  res.base() = base();
693  res.type = type;
694  res.joint1_id = joint1_id;
695  res.joint2_id = joint2_id;
696  res.joint1_placement = joint1_placement.template cast<NewScalar>();
697  res.joint2_placement = joint2_placement.template cast<NewScalar>();
698  res.reference_frame = reference_frame;
699  res.desired_contact_placement = desired_contact_placement.template cast<NewScalar>();
700  res.desired_contact_velocity = desired_contact_velocity.template cast<NewScalar>();
701  res.desired_contact_acceleration = desired_contact_acceleration.template cast<NewScalar>();
702  res.corrector = corrector.template cast<NewScalar>();
703  res.colwise_joint1_sparsity = colwise_joint1_sparsity;
704  res.colwise_joint2_sparsity = colwise_joint2_sparsity;
705  res.nv = nv;
706  res.depth_joint1 = depth_joint1;
707  res.depth_joint2 = depth_joint2;
708  res.loop_span_indexes = loop_span_indexes;
709 
710  return res;
711  }
712 
713  protected:
714  template<int OtherOptions, template<typename, int> class JointCollectionTpl>
715  void init(const ModelTpl<Scalar, OtherOptions, JointCollectionTpl> & model)
716  {
717  PINOCCHIO_CHECK_INPUT_ARGUMENT(
718  reference_frame == LOCAL || reference_frame == LOCAL_WORLD_ALIGNED,
719  "reference_frame should be LOCAL or LOCAL_WORLD_ALIGNED");
720  nv = model.nv;
721  depth_joint1 = static_cast<size_t>(model.supports[joint1_id].size());
722  depth_joint2 = static_cast<size_t>(model.supports[joint2_id].size());
723 
724  typedef ModelTpl<Scalar, OtherOptions, JointCollectionTpl> Model;
725  typedef typename Model::JointModel JointModel;
726  static const bool default_sparsity_value = false;
727  colwise_joint1_sparsity.fill(default_sparsity_value);
728  colwise_joint2_sparsity.fill(default_sparsity_value);
729  joint1_span_indexes.reserve((size_t)model.njoints);
730  joint2_span_indexes.reserve((size_t)model.njoints);
731 
732  JointIndex current1_id = 0;
733  if (joint1_id > 0)
734  current1_id = joint1_id;
735 
736  JointIndex current2_id = 0;
737  if (joint2_id > 0)
738  current2_id = joint2_id;
739 
740  while (current1_id != current2_id)
741  {
742  if (current1_id > current2_id)
743  {
744  const JointModel & joint1 = model.joints[current1_id];
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 < joint1.nv(); ++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  joint2_span_indexes.push_back((Eigen::DenseIndex)current2_id);
757  Eigen::DenseIndex current2_col_id = joint2.idx_v();
758  for (int k = 0; k < joint2.nv(); ++k, ++current2_col_id)
759  {
760  colwise_joint2_sparsity[current2_col_id] = true;
761  }
762  current2_id = model.parents[current2_id];
763  }
764  }
765  assert(current1_id == current2_id && "current1_id should be equal to current2_id");
766 
767  if (type == CONTACT_3D)
768  {
769  JointIndex current_id = current1_id;
770  while (current_id > 0)
771  {
772  const JointModel & joint = model.joints[current_id];
773  joint1_span_indexes.push_back((Eigen::DenseIndex)current_id);
774  joint2_span_indexes.push_back((Eigen::DenseIndex)current_id);
775  Eigen::DenseIndex current_row_id = joint.idx_v();
776  for (int k = 0; k < joint.nv(); ++k, ++current_row_id)
777  {
778  colwise_joint1_sparsity[current_row_id] = true;
779  colwise_joint2_sparsity[current_row_id] = true;
780  }
781  current_id = model.parents[current_id];
782  }
783  }
784 
785  std::rotate(
786  joint1_span_indexes.rbegin(), joint1_span_indexes.rbegin() + 1, joint1_span_indexes.rend());
787  std::rotate(
788  joint2_span_indexes.rbegin(), joint2_span_indexes.rbegin() + 1, joint2_span_indexes.rend());
789  Base::colwise_span_indexes.reserve((size_t)model.nv);
790  loop_span_indexes.reserve((size_t)model.nv);
791  for (Eigen::DenseIndex col_id = 0; col_id < model.nv; ++col_id)
792  {
793  if (colwise_joint1_sparsity[col_id] || colwise_joint2_sparsity[col_id])
794  {
795  colwise_span_indexes.push_back(col_id);
796  colwise_sparsity[col_id] = true;
797  }
798 
799  if (colwise_joint1_sparsity[col_id] != colwise_joint2_sparsity[col_id])
800  {
801  loop_span_indexes.push_back(col_id);
802  }
803  }
804  }
805  };
806 
807  template<typename Scalar, int Options, class Allocator>
808  size_t getTotalConstraintSize(
809  const std::vector<RigidConstraintModelTpl<Scalar, Options>, Allocator> & contact_models)
810  {
811  typedef std::vector<RigidConstraintModelTpl<Scalar, Options>, Allocator> VectorType;
812  size_t total_size = 0;
813  for (typename VectorType::const_iterator it = contact_models.begin();
814  it != contact_models.end(); ++it)
815  total_size += it->size();
816 
817  return total_size;
818  }
819 
823  template<typename _Scalar, int _Options>
824  struct PINOCCHIO_UNSUPPORTED_MESSAGE("The API will change towards more flexibility")
825  RigidConstraintDataTpl : ConstraintDataBase<RigidConstraintDataTpl<_Scalar, _Options>>
826  {
827  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
828 
829  typedef _Scalar Scalar;
830  enum
831  {
832  Options = _Options
833  };
834 
835  typedef RigidConstraintModelTpl<Scalar, Options> ContactModel;
836  typedef RigidConstraintDataTpl ContactData;
837 
838  typedef SE3Tpl<Scalar, Options> SE3;
839  typedef MotionTpl<Scalar, Options> Motion;
840  typedef ForceTpl<Scalar, Options> Force;
841  typedef Eigen::Matrix<Scalar, 6, 6, Options> Matrix6;
842  typedef PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(Matrix6) VectorOfMatrix6;
843  typedef Eigen::Matrix<Scalar, 6, Eigen::Dynamic, Options> Matrix6x;
844  typedef Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, Options> MatrixX;
845 
846  // data
847 
849  Force contact_force;
850 
852  SE3 oMc1;
853 
855  SE3 oMc2;
856 
858  SE3 c1Mc2;
859 
861  Motion contact_placement_error;
862 
864  Motion contact1_velocity;
865 
867  Motion contact2_velocity;
868 
870  Motion contact_velocity_error;
871 
873  Motion contact_acceleration;
874 
876  Motion contact_acceleration_desired;
877 
879  Motion contact_acceleration_error;
880 
883  Motion contact1_acceleration_drift;
884 
887  Motion contact2_acceleration_drift;
888 
890  Motion contact_acceleration_deviation;
891 
892  VectorOfMatrix6 extended_motion_propagators_joint1;
893  VectorOfMatrix6 lambdas_joint1;
894  VectorOfMatrix6 extended_motion_propagators_joint2;
895 
896  Matrix6x dv1_dq, da1_dq, da1_dv, da1_da;
897  Matrix6x dv2_dq, da2_dq, da2_dv, da2_da;
898  MatrixX dvc_dq, dac_dq, dac_dv, dac_da;
899 
901  RigidConstraintDataTpl()
902  {
903  }
904 
905  explicit RigidConstraintDataTpl(const ContactModel & contact_model)
906  : contact_force(Force::Zero())
907  , contact_placement_error(Motion::Zero())
908  , contact1_velocity(Motion::Zero())
909  , contact2_velocity(Motion::Zero())
910  , contact_velocity_error(Motion::Zero())
911  , contact_acceleration(Motion::Zero())
912  , contact_acceleration_desired(Motion::Zero())
913  , contact_acceleration_error(Motion::Zero())
914  , contact1_acceleration_drift(Motion::Zero())
915  , contact2_acceleration_drift(Motion::Zero())
916  , contact_acceleration_deviation(Motion::Zero())
917  , extended_motion_propagators_joint1(contact_model.depth_joint1, Matrix6::Zero())
918  , lambdas_joint1(contact_model.depth_joint1, Matrix6::Zero())
919  , extended_motion_propagators_joint2(contact_model.depth_joint2, Matrix6::Zero())
920  , dv1_dq(6, contact_model.nv)
921  , da1_dq(6, contact_model.nv)
922  , da1_dv(6, contact_model.nv)
923  , da1_da(6, contact_model.nv)
924  , dv2_dq(6, contact_model.nv)
925  , da2_dq(6, contact_model.nv)
926  , da2_dv(6, contact_model.nv)
927  , da2_da(6, contact_model.nv)
928  , dvc_dq(contact_model.size(), contact_model.nv)
929  , dac_dq(contact_model.size(), contact_model.nv)
930  , dac_dv(contact_model.size(), contact_model.nv)
931  , dac_da(contact_model.size(), contact_model.nv)
932  {
933  }
934 
935  bool operator==(const RigidConstraintDataTpl & other) const
936  {
937  return contact_force == other.contact_force && oMc1 == other.oMc1 && oMc2 == other.oMc2
938  && c1Mc2 == other.c1Mc2 && contact_placement_error == other.contact_placement_error
939  && contact1_velocity == other.contact1_velocity
940  && contact2_velocity == other.contact2_velocity
941  && contact_velocity_error == other.contact_velocity_error
942  && contact_acceleration == other.contact_acceleration
943  && contact_acceleration_desired == other.contact_acceleration_desired
944  && contact_acceleration_error == other.contact_acceleration_error
945  && contact1_acceleration_drift == other.contact1_acceleration_drift
946  && contact2_acceleration_drift == other.contact2_acceleration_drift
947  && contact_acceleration_deviation == other.contact_acceleration_deviation
948  && extended_motion_propagators_joint1 == other.extended_motion_propagators_joint1
949  && lambdas_joint1 == other.lambdas_joint1
950  && extended_motion_propagators_joint2 == other.extended_motion_propagators_joint2
951  //
952  && dv1_dq == other.dv1_dq && da1_dq == other.da1_dq && da1_dv == other.da1_dv
953  && da1_da == other.da1_da
954  //
955  && dv2_dq == other.dv2_dq && da2_dq == other.da2_dq && da2_dv == other.da2_dv
956  && da2_da == other.da2_da
957  //
958  && dvc_dq == other.dvc_dq && dac_dq == other.dac_dq && dac_dv == other.dac_dv
959  && dac_da == other.dac_da;
960  }
961 
962  bool operator!=(const RigidConstraintDataTpl & other) const
963  {
964  return !(*this == other);
965  }
966  };
967  PINOCCHIO_COMPILER_DIAGNOSTIC_POP
968 
969 } // namespace pinocchio
970 
971 #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:808
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