pinocchio  2.2.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  ScaledConstraint(const Constraint & constraint,
75  const Scalar & scaling_factor)
76  : m_constraint(constraint)
77  , m_scaling_factor(scaling_factor)
78  {}
79 
80  ScaledConstraint & operator=(const ScaledConstraint & other)
81  {
82  m_constraint = other.m_constraint;
83  m_scaling_factor = other.m_scaling_factor;
84  return *this;
85  }
86 
87  template<typename VectorLike>
88  JointMotion __mult__(const Eigen::MatrixBase<VectorLike> & v) const
89  {
90  assert(v.size() == nv());
91  JointMotion jm = m_constraint * v;
92  return jm * m_scaling_factor;
93  }
94 
95  template<typename S1, int O1>
96  SE3ActionReturnType
97  se3Action(const SE3Tpl<S1,O1> & m) const
98  {
99  SE3ActionReturnType res = m_constraint.se3Action(m);
100  return m_scaling_factor * res;
101  }
102 
103  template<typename S1, int O1>
104  SE3ActionReturnType
105  se3ActionInverse(const SE3Tpl<S1,O1> & m) const
106  {
107  SE3ActionReturnType res = m_constraint.se3ActionInverse(m);
108  return m_scaling_factor * res;
109  }
110 
111  int nv_impl() const { return m_constraint.nv(); }
112 
114  {
115  const ScaledConstraint & ref;
116  TransposeConst(const ScaledConstraint & ref) : ref(ref) {}
117 
118  template<typename Derived>
119  typename ConstraintForceOp<ScaledConstraint,Derived>::ReturnType
120  operator*(const ForceDense<Derived> & f) const
121  {
122  // TODO: I don't know why, but we should a dense a return type, otherwise it failes at the evaluation level;
123  typedef typename ConstraintForceOp<ScaledConstraint,Derived>::ReturnType ReturnType;
124  return ReturnType(ref.m_scaling_factor * (ref.m_constraint.transpose() * f));
125  }
126 
128  template<typename Derived>
129  typename ConstraintForceSetOp<ScaledConstraint,Derived>::ReturnType
130  operator*(const Eigen::MatrixBase<Derived> & F) const
131  {
132  typedef typename ConstraintForceSetOp<ScaledConstraint,Derived>::ReturnType ReturnType;
133  return ReturnType(ref.m_scaling_factor * (ref.m_constraint.transpose() * F));
134  }
135 
136  }; // struct TransposeConst
137 
138  TransposeConst transpose() const { return TransposeConst(*this); }
139 
140  DenseBase matrix_impl() const
141  {
142  DenseBase S = m_scaling_factor * m_constraint.matrix();
143  return S;
144  }
145 
146  template<typename MotionDerived>
148  motionAction(const MotionDense<MotionDerived> & m) const
149  {
151  ReturnType res = m_scaling_factor * m_constraint.motionAction(m);
152  return res;
153  }
154 
155  inline const Scalar & scaling() const { return m_scaling_factor; }
156  inline const Constraint & constraint() const { return m_constraint; }
157 
158  protected:
159 
160  Constraint m_constraint;
161  Scalar m_scaling_factor;
162  }; // struct ScaledConstraint
163 
164  template<typename S1, int O1, typename _Constraint>
165  struct MultiplicationOp<InertiaTpl<S1,O1>, ScaledConstraint<_Constraint> >
166  {
167  typedef InertiaTpl<S1,O1> Inertia;
169  typedef typename Constraint::Scalar Scalar;
170 
171  typedef typename MultiplicationOp<Inertia,_Constraint>::ReturnType OriginalReturnType;
172 // typedef typename ScalarMatrixProduct<Scalar,OriginalReturnType>::type ReturnType;
173  typedef OriginalReturnType ReturnType;
174  };
175 
176  /* [CRBA] ForceSet operator* (Inertia Y,Constraint S) */
177  namespace impl
178  {
179  template<typename S1, int O1, typename _Constraint>
180  struct LhsMultiplicationOp<InertiaTpl<S1,O1>, ScaledConstraint<_Constraint> >
181  {
182  typedef InertiaTpl<S1,O1> Inertia;
184  typedef typename MultiplicationOp<Inertia,Constraint>::ReturnType ReturnType;
185 
186  static inline ReturnType run(const Inertia & Y,
187  const Constraint & scaled_constraint)
188  {
189  return scaled_constraint.scaling() * (Y * scaled_constraint.constraint());
190  }
191  };
192  } // namespace impl
193 
194  template<typename M6Like, typename _Constraint>
195  struct MultiplicationOp<Eigen::MatrixBase<M6Like>, ScaledConstraint<_Constraint> >
196  {
197  typedef typename MultiplicationOp<Inertia,_Constraint>::ReturnType OriginalReturnType;
198  typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(OriginalReturnType) ReturnType;
199  };
200 
201  /* [ABA] operator* (Inertia Y,Constraint S) */
202  namespace impl
203  {
204  template<typename M6Like, typename _Constraint>
205  struct LhsMultiplicationOp<Eigen::MatrixBase<M6Like>, ScaledConstraint<_Constraint> >
206  {
208  typedef typename MultiplicationOp<Eigen::MatrixBase<M6Like>,Constraint>::ReturnType ReturnType;
209 
210  static inline ReturnType run(const Eigen::MatrixBase<M6Like> & Y,
211  const Constraint & scaled_constraint)
212  {
213  return scaled_constraint.scaling() * (Y.derived() * scaled_constraint.constraint());
214  }
215  };
216  } // namespace impl
217 
218  template<class Joint> struct JointMimic;
219  template<class JointModel> struct JointModelMimic;
220  template<class JointData> struct JointDataMimic;
221 
222  template<class Joint>
224  {
225  enum
226  {
227  NQ = traits<Joint>::NV,
228  NV = traits<Joint>::NQ
229  };
230  typedef typename traits<Joint>::Scalar Scalar;
231  enum { Options = traits<Joint>::Options };
232 
233  typedef typename traits<Joint>::JointDataDerived JointDataBase;
234  typedef typename traits<Joint>::JointModelDerived JointModelBase;
235 
238 
240  typedef typename traits<Joint>::Transformation_t Transformation_t;
241  typedef typename traits<Joint>::Motion_t Motion_t;
242  typedef typename traits<Joint>::Bias_t Bias_t;
243 
244  // [ABA]
245  typedef typename traits<Joint>::U_t U_t;
246  typedef typename traits<Joint>::D_t D_t;
247  typedef typename traits<Joint>::UD_t UD_t;
248 
249  PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE
250 
251  typedef typename traits<Joint>::ConfigVector_t ConfigVector_t;
252  typedef typename traits<Joint>::TangentVector_t TangentVector_t;
253  };
254 
255  template<class Joint>
258 
259  template<class Joint>
262 
263  template<class JointData>
264  struct JointDataMimic
265  : public JointDataBase< JointDataMimic<JointData> >
266  {
267  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
268 
271 
272  PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(JointDerived);
273 
274  JointDataMimic() {}
275 
277  const Scalar & scaling)
278  : jdata_ref(jdata.derived())
279  , scaling(scaling)
280  , S(jdata_ref.S,scaling)
281  {}
282 
283  JointDataMimic & operator=(const JointDataMimic & other)
284  {
285  jdata_ref = other.jdata_ref;
286  scaling = other.scaling;
287  S = Constraint_t(jdata_ref.S,other.scaling);
288  return *this;
289  }
290 
291  static std::string classname()
292  {
293  return std::string("JointDataMimic<") + JointData::classname() + std::string(">");
294  }
295 
296  std::string shortname() const
297  {
298  return std::string("JointDataMimic<") + jdata_ref.shortname() + std::string(">");
299  }
300 
301  // Accessors
302  ConstraintTypeConstRef S_accessor() const { return S; }
303  TansformTypeConstRef M_accessor() const { return jdata_ref.M; }
304  MotionTypeConstRef v_accessor() const { return jdata_ref.v; }
305  BiasTypeConstRef c_accessor() const { return jdata_ref.c; }
306  UTypeConstRef U_accessor() const { return jdata_ref.U; }
307  UTypeRef U_accessor() { return jdata_ref.U; }
308  DTypeConstRef Dinv_accessor() const { return jdata_ref.Dinv; }
309  UDTypeConstRef UDinv_accessor() const { return jdata_ref.UDinv; }
310 
311  template<class JointModel>
312  friend struct JointModelMimic;
313 
314  protected:
315 
316  JointData jdata_ref;
317  Scalar scaling;
318 
320  ConfigVector_t q_transform;
322  TangentVector_t v_transform;
323 
324  public:
325 
326  // data
327  Constraint_t S;
328 
329  }; // struct JointDataMimic
330 
331  template<typename NewScalar, typename JointModel>
332  struct CastType< NewScalar, JointModelMimic<JointModel> >
333  {
334  typedef typename CastType<NewScalar,JointModel>::type JointModelNewType;
336  };
337 
338  template<class JointModel>
339  struct JointModelMimic
340  : public JointModelBase< JointModelMimic<JointModel> >
341  {
342  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
343 
344  typedef typename traits<JointModelMimic>::JointDerived JointDerived;
345 
346  PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived);
347 
348  typedef JointModelBase<JointModelMimic> Base;
349  using Base::id;
350  using Base::idx_q;
351  using Base::idx_v;
352  using Base::nq;
353  using Base::nv;
354  using Base::setIndexes;
355 
357  {}
358 
360  const Scalar & scaling,
361  const Scalar & offset)
362  : m_jmodel_ref(jmodel.derived())
363  , m_scaling(scaling)
364  , m_offset(offset)
365  {}
366 
367  Base & base() { return *static_cast<Base*>(this); }
368  const Base & base() const { return *static_cast<const Base*>(this); }
369 
370  inline int nq_impl() const { return 0; }
371  inline int nv_impl() const { return 0; }
372 
373  inline int idx_q_impl() const { return m_jmodel_ref.idx_q(); }
374  inline int idx_v_impl() const { return m_jmodel_ref.idx_v(); }
375 
376  void setIndexes_impl(JointIndex id, int /*q*/, int /*v*/)
377  {
378  Base::i_id = id; // Only the id of the joint in the model is different.
379  Base::i_q = m_jmodel_ref.idx_q();
380  Base::i_v = m_jmodel_ref.idx_v();
381  }
382 
383  JointDataDerived createData() const
384  {
385  return JointDataDerived(m_jmodel_ref.createData(),scaling());
386  }
387 
388  template<typename ConfigVector>
389  EIGEN_DONT_INLINE
390  void calc(JointDataDerived & jdata,
391  const typename Eigen::MatrixBase<ConfigVector> & qs) const
392  {
393  typedef typename ConfigVectorAffineTransform<JointDerived>::Type AffineTransform;
394 
395  AffineTransform::run(qs.head(m_jmodel_ref.nq()),
396  m_scaling,m_offset,jdata.q_transform);
397  m_jmodel_ref.calc(jdata.jdata_ref,jdata.q_transform);
398  }
399 
400  template<typename ConfigVector, typename TangentVector>
401  EIGEN_DONT_INLINE
402  void calc(JointDataDerived & jdata,
403  const typename Eigen::MatrixBase<ConfigVector> & qs,
404  const typename Eigen::MatrixBase<TangentVector> & vs) const
405  {
406  typedef typename ConfigVectorAffineTransform<JointDerived>::Type AffineTransform;
407 
408  AffineTransform::run(qs.head(m_jmodel_ref.nq()),
409  m_scaling,m_offset,jdata.q_transform);
410  jdata.v_transform = m_scaling * vs.head(m_jmodel_ref.nv());
411  m_jmodel_ref.calc(jdata.jdata_ref,
412  jdata.q_transform,
413  jdata.v_transform);
414  }
415 
416  template<typename Matrix6Like>
417  void calc_aba(JointDataDerived & data,
418  const Eigen::MatrixBase<Matrix6Like> & I,
419  const bool update_I) const
420  {
421  m_jmodel_ref.calc_aba(data.jdata_ref,
422  PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like,I),
423  update_I);
424  }
425 
426  static std::string classname()
427  {
428  return std::string("JointModelMimic<") + JointModel::classname() + std::string(">");;
429  }
430 
431  std::string shortname() const
432  {
433  return std::string("JointModelMimic<") + m_jmodel_ref.shortname() + std::string(">");
434  }
435 
437  template<typename NewScalar>
439  {
440  typedef typename CastType<NewScalar,JointModelMimic>::type ReturnType;
441 
442  ReturnType res(m_jmodel_ref.template cast<NewScalar>(),
443  (NewScalar)m_scaling,
444  (NewScalar)m_offset);
445  res.setIndexes(id(),idx_q(),idx_v());
446  return res;
447  }
448 
449  const JointModel & jmodel() const { return m_jmodel_ref; }
450  JointModel & jmodel() { return m_jmodel_ref; }
451 
452  const Scalar & scaling() const { return m_scaling; }
453  Scalar & scaling() { return m_scaling; }
454 
455  const Scalar & offset() const { return m_offset; }
456  Scalar & offset() { return m_offset; }
457 
458  protected:
459 
460  // data
461  JointModel m_jmodel_ref;
462  Scalar m_scaling, m_offset;
463 
464  public:
465 
466  /* Acces to dedicated segment in robot config space. */
467  // Const access
468  template<typename D>
469  typename SizeDepType<NQ>::template SegmentReturn<D>::ConstType
470  jointConfigSelector_impl(const Eigen::MatrixBase<D> & a) const
471  {
472  return SizeDepType<NQ>::segment(a.derived(),
473  m_jmodel_ref.idx_q(),
474  m_jmodel_ref.nq());
475  }
476 
477  // Non-const access
478  template<typename D>
479  typename SizeDepType<NQ>::template SegmentReturn<D>::Type
480  jointConfigSelector_impl(Eigen::MatrixBase<D> & a) const
481  {
482  return SizeDepType<NQ>::segment(a.derived(),
483  m_jmodel_ref.idx_q(),
484  m_jmodel_ref.nq());
485  }
486 
487  /* Acces to dedicated segment in robot config velocity space. */
488  // Const access
489  template<typename D>
490  typename SizeDepType<NV>::template SegmentReturn<D>::ConstType
491  jointVelocitySelector_impl(const Eigen::MatrixBase<D> & a) const
492  {
493  return SizeDepType<NV>::segment(a.derived(),
494  m_jmodel_ref.idx_v(),
495  m_jmodel_ref.nv());
496  }
497 
498  // Non-const access
499  template<typename D>
500  typename SizeDepType<NV>::template SegmentReturn<D>::Type
501  jointVelocitySelector_impl(Eigen::MatrixBase<D> & a) const
502  {
503  return SizeDepType<NV>::segment(a.derived(),
504  m_jmodel_ref.idx_v(),
505  m_jmodel_ref.nv());
506  }
507 
508  /* Acces to dedicated columns in a ForceSet or MotionSet matrix.*/
509  // Const access
510  template<typename D>
511  typename SizeDepType<NV>::template ColsReturn<D>::ConstType
512  jointCols_impl(const Eigen::MatrixBase<D> & A) const
513  {
514  return SizeDepType<NV>::middleCols(A.derived(),
515  m_jmodel_ref.idx_v(),
516  m_jmodel_ref.nv());
517  }
518 
519  // Non-const access
520  template<typename D>
521  typename SizeDepType<NV>::template ColsReturn<D>::Type
522  jointCols_impl(Eigen::MatrixBase<D> & A) const
523  {
524  return SizeDepType<NV>::middleCols(A.derived(),
525  m_jmodel_ref.idx_v(),
526  m_jmodel_ref.nv());
527  }
528 
529  /* Acces to dedicated rows in a matrix.*/
530  // Const access
531  template<typename D>
532  typename SizeDepType<NV>::template RowsReturn<D>::ConstType
533  jointRows_impl(const Eigen::MatrixBase<D> & A) const
534  {
535  return SizeDepType<NV>::middleRows(A.derived(),
536  m_jmodel_ref.idx_v(),
537  m_jmodel_ref.nv());
538  }
539 
540  // Non-const access
541  template<typename D>
542  typename SizeDepType<NV>::template RowsReturn<D>::Type
543  jointRows_impl(Eigen::MatrixBase<D> & A) const
544  {
545  return SizeDepType<NV>::middleRows(A.derived(),
546  m_jmodel_ref.idx_v(),
547  m_jmodel_ref.nv());
548  }
549 
551  // Const access
552  template<typename D>
553  typename SizeDepType<NV>::template BlockReturn<D>::ConstType
554  jointBlock_impl(const Eigen::MatrixBase<D> & Mat) const
555  {
556  return SizeDepType<NV>::block(Mat.derived(),
557  m_jmodel_ref.idx_v(),m_jmodel_ref.idx_v(),
558  m_jmodel_ref.nv(),m_jmodel_ref.nv());
559  }
560 
561  // Non-const access
562  template<typename D>
563  typename SizeDepType<NV>::template BlockReturn<D>::Type
564  jointBlock_impl(Eigen::MatrixBase<D> & Mat) const
565  {
566  return SizeDepType<NV>::block(Mat.derived(),
567  m_jmodel_ref.idx_v(),m_jmodel_ref.idx_v(),
568  m_jmodel_ref.nv(),m_jmodel_ref.nv());
569  }
570 
571  }; // struct JointModelMimic
572 
573 } // namespace pinocchio
574 
575 #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...
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...
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>
CastType< NewScalar, JointModelMimic >::type cast() const
ConstraintForceSetOp< ScaledConstraint, Derived >::ReturnType operator*(const Eigen::MatrixBase< Derived > &F) const
[CRBA] MatrixBase operator* (Constraint::Transpose S, ForceSet::Block)
std::string shortname(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointShortnameVisitor to get the shortname of the derived joint model...
Main pinocchio namespace.
Definition: treeview.dox:24
Common traits structure to fully define base classes for CRTP.
Definition: spatial/fwd.hpp:32
TangentVector_t v_transform
Transform velocity vector.
ConfigVector_t q_transform
Transform configuration vector.
Type of the cast of a class C templated by Scalar and Options, to a new NewScalar type...
Definition: spatial/fwd.hpp:43
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)
 .