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