pinocchio  2.7.1
A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
joint-mimic.hpp
1 //
2 // Copyright (c) 2019-2021 INRIA
3 //
4 
5 #ifndef __pinocchio_multibody_joint_mimic_hpp__
6 #define __pinocchio_multibody_joint_mimic_hpp__
7 
8 #include "pinocchio/macros.hpp"
9 #include "pinocchio/multibody/joint/joint-base.hpp"
10 
11 namespace pinocchio
12 {
13 
14  template<class Constraint> struct ScaledConstraint;
15 
16  template<class Constraint>
17  struct traits< ScaledConstraint<Constraint> >
18  {
19  typedef typename traits<Constraint>::Scalar Scalar;
20  enum { Options = traits<Constraint>::Options };
21  enum {
22  LINEAR = traits<Constraint>::LINEAR,
23  ANGULAR = traits<Constraint>::ANGULAR
24  };
25  typedef typename traits<Constraint>::JointMotion JointMotion;
26  typedef typename traits<Constraint>::JointForce JointForce;
27  typedef typename traits<Constraint>::DenseBase DenseBase;
28  typedef typename traits<Constraint>::MatrixReturnType MatrixReturnType;
29  typedef typename traits<Constraint>::ConstMatrixReturnType ConstMatrixReturnType;
30  }; // traits ScaledConstraint
31 
32  template<class Constraint>
33  struct SE3GroupAction< ScaledConstraint<Constraint> >
34  { typedef typename SE3GroupAction<Constraint>::ReturnType ReturnType; };
35 
36  template<class Constraint, typename MotionDerived>
37  struct MotionAlgebraAction< ScaledConstraint<Constraint>, MotionDerived >
38  { typedef typename MotionAlgebraAction<Constraint,MotionDerived>::ReturnType ReturnType; };
39 
40  template<class Constraint, typename ForceDerived>
41  struct ConstraintForceOp< ScaledConstraint<Constraint>, ForceDerived>
42  {
43  typedef typename Constraint::Scalar Scalar;
44  typedef typename ConstraintForceOp<Constraint,ForceDerived>::ReturnType OriginalReturnType;
45 
46  typedef typename ScalarMatrixProduct<Scalar,OriginalReturnType>::type IdealReturnType;
47  typedef Eigen::Matrix<Scalar,IdealReturnType::RowsAtCompileTime,IdealReturnType::ColsAtCompileTime,Constraint::Options> ReturnType;
48  };
49 
50  template<class Constraint, typename ForceSet>
51  struct ConstraintForceSetOp< ScaledConstraint<Constraint>, ForceSet>
52  {
53  typedef typename Constraint::Scalar Scalar;
54  typedef typename ConstraintForceSetOp<Constraint,ForceSet>::ReturnType OriginalReturnType;
55  typedef typename ScalarMatrixProduct<Scalar,OriginalReturnType>::type IdealReturnType;
56  typedef Eigen::Matrix<Scalar,Constraint::NV,ForceSet::ColsAtCompileTime,Constraint::Options | Eigen::RowMajor> ReturnType;
57  };
58 
59  template<class Constraint>
60  struct ScaledConstraint
61  : ConstraintBase< ScaledConstraint<Constraint> >
62  {
63  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
64 
65  PINOCCHIO_CONSTRAINT_TYPEDEF_TPL(ScaledConstraint)
66  enum { NV = Constraint::NV };
67  typedef ConstraintBase<ScaledConstraint> Base;
68  using Base::nv;
69 
70  typedef typename SE3GroupAction<Constraint>::ReturnType SE3ActionReturnType;
71 
72  ScaledConstraint() {}
73 
74  explicit ScaledConstraint(const Scalar & scaling_factor)
75  : m_scaling_factor(scaling_factor)
76  {}
77 
78  ScaledConstraint(const Constraint & constraint,
79  const Scalar & scaling_factor)
80  : m_constraint(constraint)
81  , m_scaling_factor(scaling_factor)
82  {}
83 
84  ScaledConstraint(const ScaledConstraint & other)
85  : m_constraint(other.m_constraint)
86  , m_scaling_factor(other.m_scaling_factor)
87  {}
88 
89  ScaledConstraint & operator=(const ScaledConstraint & other)
90  {
91  m_constraint = other.m_constraint;
92  m_scaling_factor = other.m_scaling_factor;
93  return *this;
94  }
95 
96  template<typename VectorLike>
97  JointMotion __mult__(const Eigen::MatrixBase<VectorLike> & v) const
98  {
99  assert(v.size() == nv());
100  JointMotion jm = m_constraint * v;
101  return jm * m_scaling_factor;
102  }
103 
104  template<typename S1, int O1>
105  SE3ActionReturnType
106  se3Action(const SE3Tpl<S1,O1> & m) const
107  {
108  SE3ActionReturnType res = m_constraint.se3Action(m);
109  return m_scaling_factor * res;
110  }
111 
112  template<typename S1, int O1>
113  SE3ActionReturnType
114  se3ActionInverse(const SE3Tpl<S1,O1> & m) const
115  {
116  SE3ActionReturnType res = m_constraint.se3ActionInverse(m);
117  return m_scaling_factor * res;
118  }
119 
120  int nv_impl() const { return m_constraint.nv(); }
121 
122  struct TransposeConst
123  {
124  const ScaledConstraint & ref;
125  TransposeConst(const ScaledConstraint & ref) : ref(ref) {}
126 
127  template<typename Derived>
128  typename ConstraintForceOp<ScaledConstraint,Derived>::ReturnType
129  operator*(const ForceDense<Derived> & f) const
130  {
131  // TODO: I don't know why, but we should a dense a return type, otherwise it failes at the evaluation level;
132  typedef typename ConstraintForceOp<ScaledConstraint,Derived>::ReturnType ReturnType;
133  return ReturnType(ref.m_scaling_factor * (ref.m_constraint.transpose() * f));
134  }
135 
137  template<typename Derived>
138  typename ConstraintForceSetOp<ScaledConstraint,Derived>::ReturnType
139  operator*(const Eigen::MatrixBase<Derived> & F) const
140  {
141  typedef typename ConstraintForceSetOp<ScaledConstraint,Derived>::ReturnType ReturnType;
142  return ReturnType(ref.m_scaling_factor * (ref.m_constraint.transpose() * F));
143  }
144 
145  }; // struct TransposeConst
146 
147  TransposeConst transpose() const { return TransposeConst(*this); }
148 
149  DenseBase matrix_impl() const
150  {
151  DenseBase S = m_scaling_factor * m_constraint.matrix();
152  return S;
153  }
154 
155  template<typename MotionDerived>
156  typename MotionAlgebraAction<ScaledConstraint,MotionDerived>::ReturnType
157  motionAction(const MotionDense<MotionDerived> & m) const
158  {
159  typedef typename MotionAlgebraAction<ScaledConstraint,MotionDerived>::ReturnType ReturnType;
160  ReturnType res = m_scaling_factor * m_constraint.motionAction(m);
161  return res;
162  }
163 
164  inline const Scalar & scaling() const { return m_scaling_factor; }
165  inline Scalar & scaling() { return m_scaling_factor; }
166 
167  inline const Constraint & constraint() const { return m_constraint; }
168  inline Constraint & constraint() { return m_constraint; }
169 
170  bool isEqual(const ScaledConstraint & other) const
171  {
172  return m_constraint == other.m_constraint
173  && m_scaling_factor == other.m_scaling_factor;
174  }
175 
176  protected:
177 
178  Constraint m_constraint;
179  Scalar m_scaling_factor;
180  }; // struct ScaledConstraint
181 
182  template<typename S1, int O1, typename _Constraint>
183  struct MultiplicationOp<InertiaTpl<S1,O1>, ScaledConstraint<_Constraint> >
184  {
185  typedef InertiaTpl<S1,O1> Inertia;
186  typedef ScaledConstraint<_Constraint> Constraint;
187  typedef typename Constraint::Scalar Scalar;
188 
189  typedef typename MultiplicationOp<Inertia,_Constraint>::ReturnType OriginalReturnType;
190 // typedef typename ScalarMatrixProduct<Scalar,OriginalReturnType>::type ReturnType;
191  typedef OriginalReturnType ReturnType;
192  };
193 
194  /* [CRBA] ForceSet operator* (Inertia Y,Constraint S) */
195  namespace impl
196  {
197  template<typename S1, int O1, typename _Constraint>
198  struct LhsMultiplicationOp<InertiaTpl<S1,O1>, ScaledConstraint<_Constraint> >
199  {
200  typedef InertiaTpl<S1,O1> Inertia;
201  typedef ScaledConstraint<_Constraint> Constraint;
202  typedef typename MultiplicationOp<Inertia,Constraint>::ReturnType ReturnType;
203 
204  static inline ReturnType run(const Inertia & Y,
205  const Constraint & scaled_constraint)
206  {
207  return scaled_constraint.scaling() * (Y * scaled_constraint.constraint());
208  }
209  };
210  } // namespace impl
211 
212  template<typename M6Like, typename _Constraint>
213  struct MultiplicationOp<Eigen::MatrixBase<M6Like>, ScaledConstraint<_Constraint> >
214  {
215  typedef typename MultiplicationOp<Inertia,_Constraint>::ReturnType OriginalReturnType;
216  typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(OriginalReturnType) ReturnType;
217  };
218 
219  /* [ABA] operator* (Inertia Y,Constraint S) */
220  namespace impl
221  {
222  template<typename M6Like, typename _Constraint>
223  struct LhsMultiplicationOp<Eigen::MatrixBase<M6Like>, ScaledConstraint<_Constraint> >
224  {
225  typedef ScaledConstraint<_Constraint> Constraint;
226  typedef typename MultiplicationOp<Eigen::MatrixBase<M6Like>,Constraint>::ReturnType ReturnType;
227 
228  static inline ReturnType run(const Eigen::MatrixBase<M6Like> & Y,
229  const Constraint & scaled_constraint)
230  {
231  return scaled_constraint.scaling() * (Y.derived() * scaled_constraint.constraint());
232  }
233  };
234  } // namespace impl
235 
236  template<class Joint> struct JointMimic;
237  template<class JointModel> struct JointModelMimic;
238  template<class JointData> struct JointDataMimic;
239 
240  template<class Joint>
241  struct traits< JointMimic<Joint> >
242  {
243  enum
244  {
245  NQ = traits<Joint>::NV,
246  NV = traits<Joint>::NQ
247  };
248  typedef typename traits<Joint>::Scalar Scalar;
249  enum { Options = traits<Joint>::Options };
250 
251  typedef typename traits<Joint>::JointDataDerived JointDataBase;
252  typedef typename traits<Joint>::JointModelDerived JointModelBase;
253 
254  typedef JointDataMimic<JointDataBase> JointDataDerived;
255  typedef JointModelMimic<JointModelBase> JointModelDerived;
256 
257  typedef ScaledConstraint<typename traits<Joint>::Constraint_t> Constraint_t;
258  typedef typename traits<Joint>::Transformation_t Transformation_t;
259  typedef typename traits<Joint>::Motion_t Motion_t;
260  typedef typename traits<Joint>::Bias_t Bias_t;
261 
262  // [ABA]
263  typedef typename traits<Joint>::U_t U_t;
264  typedef typename traits<Joint>::D_t D_t;
265  typedef typename traits<Joint>::UD_t UD_t;
266 
267  PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE
268 
269  typedef typename traits<Joint>::ConfigVector_t ConfigVector_t;
270  typedef typename traits<Joint>::TangentVector_t TangentVector_t;
271  };
272 
273  template<class Joint>
274  struct traits< JointDataMimic<Joint> >
275  { typedef JointMimic<typename traits<Joint>::JointDerived> JointDerived; };
276 
277  template<class Joint>
278  struct traits< JointModelMimic<Joint> >
279  { typedef JointMimic<typename traits<Joint>::JointDerived> JointDerived; };
280 
281  template<class JointData>
282  struct JointDataMimic
283  : public JointDataBase< JointDataMimic<JointData> >
284  {
285  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
286 
287  typedef typename traits<JointDataMimic>::JointDerived JointDerived;
288  typedef JointDataBase< JointDataMimic<JointData> > Base;
289 
290  PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(JointDerived);
291 
292  JointDataMimic()
293  : m_scaling((Scalar)0)
294  , m_q_transform(ConfigVector_t::Zero())
295  , m_v_transform(TangentVector_t::Zero())
296  , S((Scalar)0)
297  {}
298 
299  JointDataMimic(const JointDataMimic & other)
300  { *this = other; }
301 
302  JointDataMimic(const JointDataBase<JointData> & jdata,
303  const Scalar & scaling)
304  : m_jdata_ref(jdata.derived())
305  , m_scaling(scaling)
306  , S(m_jdata_ref.S,scaling)
307  {}
308 
309  JointDataMimic & operator=(const JointDataMimic & other)
310  {
311  m_jdata_ref = other.m_jdata_ref;
312  m_scaling = other.m_scaling;
313  m_q_transform = other.m_q_transform;
314  m_v_transform = other.m_v_transform;
315  S = Constraint_t(m_jdata_ref.S,other.m_scaling);
316  return *this;
317  }
318 
319  using Base::isEqual;
320  bool isEqual(const JointDataMimic & other) const
321  {
322  return Base::isEqual(other)
323  && m_jdata_ref == other.m_jdata_ref
324  && m_scaling == other.m_scaling
325  && m_q_transform == other.m_q_transform
326  && m_v_transform == other.m_v_transform
327  ;
328  }
329 
330  static std::string classname()
331  {
332  return std::string("JointDataMimic<") + JointData::classname() + std::string(">");
333  }
334 
335  std::string shortname() const
336  {
337  return std::string("JointDataMimic<") + m_jdata_ref.shortname() + std::string(">");
338  }
339 
340  // Accessors
341  ConstraintTypeConstRef S_accessor() const { return S; }
342  ConstraintTypeRef S_accessor() { return S; }
343 
344  TansformTypeConstRef M_accessor() const { return m_jdata_ref.M; }
345  TansformTypeRef M_accessor() { return m_jdata_ref.M; }
346 
347  MotionTypeConstRef v_accessor() const { return m_jdata_ref.v; }
348  MotionTypeRef v_accessor() { return m_jdata_ref.v; }
349 
350  BiasTypeConstRef c_accessor() const { return m_jdata_ref.c; }
351  BiasTypeRef c_accessor() { return m_jdata_ref.c; }
352 
353  UTypeConstRef U_accessor() const { return m_jdata_ref.U; }
354  UTypeRef U_accessor() { return m_jdata_ref.U; }
355 
356  DTypeConstRef Dinv_accessor() const { return m_jdata_ref.Dinv; }
357  DTypeRef Dinv_accessor() { return m_jdata_ref.Dinv; }
358 
359  UDTypeConstRef UDinv_accessor() const { return m_jdata_ref.UDinv; }
360  UDTypeRef UDinv_accessor() { return m_jdata_ref.UDinv; }
361 
362  template<class JointModel>
363  friend struct JointModelMimic;
364 
365  const JointData & jdata() const { return m_jdata_ref; }
366  JointData & jdata() { return m_jdata_ref; }
367 
368  const Scalar & scaling() const { return m_scaling; }
369  Scalar & scaling() { return m_scaling; }
370 
371  ConfigVector_t & jointConfiguration() { return m_q_transform; }
372  const ConfigVector_t & jointConfiguration() const { return m_q_transform; }
373 
374  TangentVector_t & jointVelocity() { return m_v_transform; }
375  const TangentVector_t & jointVelocity() const { return m_v_transform; }
376 
377  protected:
378 
379  JointData m_jdata_ref;
380  Scalar m_scaling;
381 
383  ConfigVector_t m_q_transform;
385  TangentVector_t m_v_transform;
386 
387  public:
388 
389  // data
390  Constraint_t S;
391 
392  }; // struct JointDataMimic
393 
394  template<typename NewScalar, typename JointModel>
395  struct CastType< NewScalar, JointModelMimic<JointModel> >
396  {
397  typedef typename CastType<NewScalar,JointModel>::type JointModelNewType;
398  typedef JointModelMimic<JointModelNewType> type;
399  };
400 
401  template<class JointModel>
402  struct JointModelMimic
403  : public JointModelBase< JointModelMimic<JointModel> >
404  {
405  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
406 
407  typedef typename traits<JointModelMimic>::JointDerived JointDerived;
408 
409  PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived);
410 
411  typedef JointModelBase<JointModelMimic> Base;
412  using Base::id;
413  using Base::idx_q;
414  using Base::idx_v;
415  using Base::nq;
416  using Base::nv;
417  using Base::setIndexes;
418 
419  JointModelMimic()
420  {}
421 
422  JointModelMimic(const JointModelBase<JointModel> & jmodel,
423  const Scalar & scaling,
424  const Scalar & offset)
425  : m_jmodel_ref(jmodel.derived())
426  , m_scaling(scaling)
427  , m_offset(offset)
428  {}
429 
430  Base & base() { return *static_cast<Base*>(this); }
431  const Base & base() const { return *static_cast<const Base*>(this); }
432 
433  inline int nq_impl() const { return 0; }
434  inline int nv_impl() const { return 0; }
435 
436  inline int idx_q_impl() const { return m_jmodel_ref.idx_q(); }
437  inline int idx_v_impl() const { return m_jmodel_ref.idx_v(); }
438 
439  void setIndexes_impl(JointIndex id, int /*q*/, int /*v*/)
440  {
441  Base::i_id = id; // Only the id of the joint in the model is different.
442  Base::i_q = m_jmodel_ref.idx_q();
443  Base::i_v = m_jmodel_ref.idx_v();
444  }
445 
446  JointDataDerived createData() const
447  {
448  return JointDataDerived(m_jmodel_ref.createData(),scaling());
449  }
450 
451  const std::vector<bool> hasConfigurationLimit() const
452  {
453  return m_jmodel_ref.hasConfigurationLimit();
454  }
455 
456  const std::vector<bool> hasConfigurationLimitInTangent() const
457  {
458  return m_jmodel_ref.hasConfigurationLimitInTangent();
459  }
460 
461  template<typename ConfigVector>
462  EIGEN_DONT_INLINE
463  void calc(JointDataDerived & jdata,
464  const typename Eigen::MatrixBase<ConfigVector> & qs) const
465  {
466  typedef typename ConfigVectorAffineTransform<JointDerived>::Type AffineTransform;
467 
468  AffineTransform::run(qs.head(m_jmodel_ref.nq()),
469  m_scaling,m_offset,jdata.m_q_transform);
470  m_jmodel_ref.calc(jdata.m_jdata_ref,jdata.m_q_transform);
471  }
472 
473  template<typename ConfigVector, typename TangentVector>
474  EIGEN_DONT_INLINE
475  void calc(JointDataDerived & jdata,
476  const typename Eigen::MatrixBase<ConfigVector> & qs,
477  const typename Eigen::MatrixBase<TangentVector> & vs) const
478  {
479  typedef typename ConfigVectorAffineTransform<JointDerived>::Type AffineTransform;
480 
481  AffineTransform::run(qs.head(m_jmodel_ref.nq()),
482  m_scaling,m_offset,jdata.m_q_transform);
483  jdata.m_v_transform = m_scaling * vs.head(m_jmodel_ref.nv());
484  m_jmodel_ref.calc(jdata.m_jdata_ref,
485  jdata.m_q_transform,
486  jdata.m_v_transform);
487  }
488 
489  template<typename Matrix6Like>
490  void calc_aba(JointDataDerived & data,
491  const Eigen::MatrixBase<Matrix6Like> & I,
492  const bool update_I) const
493  {
494  // TODO: fixme
495  m_jmodel_ref.calc_aba(data.m_jdata_ref,
496  PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like,I),
497  update_I);
498  }
499 
500  static std::string classname()
501  {
502  return std::string("JointModelMimic<") + JointModel::classname() + std::string(">");;
503  }
504 
505  std::string shortname() const
506  {
507  return std::string("JointModelMimic<") + m_jmodel_ref.shortname() + std::string(">");
508  }
509 
511  template<typename NewScalar>
512  typename CastType<NewScalar,JointModelMimic>::type cast() const
513  {
514  typedef typename CastType<NewScalar,JointModelMimic>::type ReturnType;
515 
516  ReturnType res(m_jmodel_ref.template cast<NewScalar>(),
517  (NewScalar)m_scaling,
518  (NewScalar)m_offset);
519  res.setIndexes(id(),idx_q(),idx_v());
520  return res;
521  }
522 
523  const JointModel & jmodel() const { return m_jmodel_ref; }
524  JointModel & jmodel() { return m_jmodel_ref; }
525 
526  const Scalar & scaling() const { return m_scaling; }
527  Scalar & scaling() { return m_scaling; }
528 
529  const Scalar & offset() const { return m_offset; }
530  Scalar & offset() { return m_offset; }
531 
532  protected:
533 
534  // data
535  JointModel m_jmodel_ref;
536  Scalar m_scaling, m_offset;
537 
538  public:
539 
540  /* Acces to dedicated segment in robot config space. */
541  // Const access
542  template<typename D>
543  typename SizeDepType<NQ>::template SegmentReturn<D>::ConstType
544  jointConfigSelector_impl(const Eigen::MatrixBase<D> & a) const
545  {
546  return SizeDepType<NQ>::segment(a.derived(),
547  m_jmodel_ref.idx_q(),
548  m_jmodel_ref.nq());
549  }
550 
551  // Non-const access
552  template<typename D>
553  typename SizeDepType<NQ>::template SegmentReturn<D>::Type
554  jointConfigSelector_impl(Eigen::MatrixBase<D> & a) const
555  {
556  return SizeDepType<NQ>::segment(a.derived(),
557  m_jmodel_ref.idx_q(),
558  m_jmodel_ref.nq());
559  }
560 
561  /* Acces to dedicated segment in robot config velocity space. */
562  // Const access
563  template<typename D>
564  typename SizeDepType<NV>::template SegmentReturn<D>::ConstType
565  jointVelocitySelector_impl(const Eigen::MatrixBase<D> & a) const
566  {
567  return SizeDepType<NV>::segment(a.derived(),
568  m_jmodel_ref.idx_v(),
569  m_jmodel_ref.nv());
570  }
571 
572  // Non-const access
573  template<typename D>
574  typename SizeDepType<NV>::template SegmentReturn<D>::Type
575  jointVelocitySelector_impl(Eigen::MatrixBase<D> & a) const
576  {
577  return SizeDepType<NV>::segment(a.derived(),
578  m_jmodel_ref.idx_v(),
579  m_jmodel_ref.nv());
580  }
581 
582  /* Acces to dedicated columns in a ForceSet or MotionSet matrix.*/
583  // Const access
584  template<typename D>
585  typename SizeDepType<NV>::template ColsReturn<D>::ConstType
586  jointCols_impl(const Eigen::MatrixBase<D> & A) const
587  {
588  return SizeDepType<NV>::middleCols(A.derived(),
589  m_jmodel_ref.idx_v(),
590  m_jmodel_ref.nv());
591  }
592 
593  // Non-const access
594  template<typename D>
595  typename SizeDepType<NV>::template ColsReturn<D>::Type
596  jointCols_impl(Eigen::MatrixBase<D> & A) const
597  {
598  return SizeDepType<NV>::middleCols(A.derived(),
599  m_jmodel_ref.idx_v(),
600  m_jmodel_ref.nv());
601  }
602 
603  /* Acces to dedicated rows in a matrix.*/
604  // Const access
605  template<typename D>
606  typename SizeDepType<NV>::template RowsReturn<D>::ConstType
607  jointRows_impl(const Eigen::MatrixBase<D> & A) const
608  {
609  return SizeDepType<NV>::middleRows(A.derived(),
610  m_jmodel_ref.idx_v(),
611  m_jmodel_ref.nv());
612  }
613 
614  // Non-const access
615  template<typename D>
616  typename SizeDepType<NV>::template RowsReturn<D>::Type
617  jointRows_impl(Eigen::MatrixBase<D> & A) const
618  {
619  return SizeDepType<NV>::middleRows(A.derived(),
620  m_jmodel_ref.idx_v(),
621  m_jmodel_ref.nv());
622  }
623 
625  // Const access
626  template<typename D>
627  typename SizeDepType<NV>::template BlockReturn<D>::ConstType
628  jointBlock_impl(const Eigen::MatrixBase<D> & Mat) const
629  {
630  return SizeDepType<NV>::block(Mat.derived(),
631  m_jmodel_ref.idx_v(),m_jmodel_ref.idx_v(),
632  m_jmodel_ref.nv(),m_jmodel_ref.nv());
633  }
634 
635  // Non-const access
636  template<typename D>
637  typename SizeDepType<NV>::template BlockReturn<D>::Type
638  jointBlock_impl(Eigen::MatrixBase<D> & Mat) const
639  {
640  return SizeDepType<NV>::block(Mat.derived(),
641  m_jmodel_ref.idx_v(),m_jmodel_ref.idx_v(),
642  m_jmodel_ref.nv(),m_jmodel_ref.nv());
643  }
644 
645  }; // struct JointModelMimic
646 
647 } // namespace pinocchio
648 
649 #endif // ifndef __pinocchio_multibody_joint_mimic_hpp__
pinocchio::SizeDepType
Definition: matrix-block.hpp:13
pinocchio::MultiplicationOp
Forward declaration of the multiplication operation return type. Should be overloaded,...
Definition: binary-op.hpp:15
pinocchio::JointDataMimic::m_q_transform
ConfigVector_t m_q_transform
Transform configuration vector.
Definition: joint-mimic.hpp:383
pinocchio::JointModelMimic
Definition: joint-mimic.hpp:237
pinocchio::JointMimic
Definition: joint-mimic.hpp:236
pinocchio::idx_q
int idx_q(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointIdxQVisitor to get the index in the full model configuration space...
pinocchio::JointModelBase
Definition: joint-model-base.hpp:67
pinocchio::ConstraintForceOp
Return type of the Constraint::Transpose * Force operation.
Definition: constraint-base.hpp:35
pinocchio::idx_v
int idx_v(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointIdxVVisitor to get the index in the full model tangent space corre...
pinocchio::JointTpl
Definition: joint-generic.hpp:19
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::id
JointIndex id(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointIdVisitor to get the index of the joint in the kinematic chain.
pinocchio::JointDataBase
Definition: joint-data-base.hpp:82
pinocchio::JointDataMimic::m_v_transform
TangentVector_t m_v_transform
Transform velocity vector.
Definition: joint-mimic.hpp:385
pinocchio::JointDataMimic
Definition: joint-mimic.hpp:238
pinocchio::MotionAlgebraAction
Return type of the ation of a Motion onto an object of type D.
Definition: motion.hpp:46
pinocchio::impl::LhsMultiplicationOp
Definition: binary-op.hpp:20
pinocchio::SE3GroupAction
Definition: se3.hpp:39
pinocchio::JointModelMimic::jointBlock_impl
SizeDepType< NV >::template BlockReturn< D >::ConstType jointBlock_impl(const Eigen::MatrixBase< D > &Mat) const
Returns a block of dimension nv()xnv() located at position idx_v(),idx_v() in the matrix Mat.
Definition: joint-mimic.hpp:628
pinocchio::ScaledConstraint::TransposeConst
Definition: joint-mimic.hpp:122
pinocchio::JointDataBase::isEqual
bool isEqual(const JointDataBase &other) const
&#160;
Definition: joint-data-base.hpp:130
pinocchio::JointModelTpl< double >
pinocchio::JointModelMimic::cast
CastType< NewScalar, JointModelMimic >::type cast() const
Definition: joint-mimic.hpp:512
pinocchio::ScaledConstraint::TransposeConst::operator*
ConstraintForceSetOp< ScaledConstraint, Derived >::ReturnType operator*(const Eigen::MatrixBase< Derived > &F) const
[CRBA] MatrixBase operator* (Constraint::Transpose S, ForceSet::Block)
Definition: joint-mimic.hpp:139
pinocchio::ForceSetTpl
Definition: force-set.hpp:14
pinocchio::ConstraintForceSetOp
Return type of the Constraint::Transpose * ForceSet operation.
Definition: constraint-base.hpp:42
pinocchio::ScalarMatrixProduct
Definition: matrix.hpp:74
pinocchio::ForceDense
Definition: force-dense.hpp:24
pinocchio::ConstraintBase
Definition: constraint-base.hpp:48
pinocchio::InertiaTpl
Definition: fwd.hpp:52
pinocchio::traits
Common traits structure to fully define base classes for CRTP.
Definition: fwd.hpp:44
pinocchio::CastType
Type of the cast of a class C templated by Scalar and Options, to a new NewScalar type....
Definition: fwd.hpp:55
pinocchio
Main pinocchio namespace.
Definition: treeview.dox:11
pinocchio::ScaledConstraint
Definition: joint-mimic.hpp:14