joint-mimic.hpp
Go to the documentation of this file.
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  {
21  enum {
24  };
30  }; // traits ScaledConstraint
31 
32  template<class Constraint>
33  struct SE3GroupAction< ScaledConstraint<Constraint> >
35 
36  template<class Constraint, typename MotionDerived>
37  struct MotionAlgebraAction< ScaledConstraint<Constraint>, MotionDerived >
39 
40  template<class Constraint, typename ForceDerived>
41  struct ConstraintForceOp< ScaledConstraint<Constraint>, ForceDerived>
42  {
43  typedef typename Constraint::Scalar Scalar;
45 
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;
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 
66  enum { NV = Constraint::NV };
68  using Base::nv;
69 
71 
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 
85  : m_constraint(other.m_constraint)
86  , m_scaling_factor(other.m_scaling_factor)
87  {}
88 
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
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 
123  {
125  TransposeConst(const ScaledConstraint & ref) : ref(ref) {}
126 
127  template<typename Derived>
130  {
131  // TODO: I don't know why, but we should a dense a return type, otherwise it failes at the evaluation level;
133  return ReturnType(ref.m_scaling_factor * (ref.m_constraint.transpose() * f));
134  }
135 
137  template<typename Derived>
139  operator*(const Eigen::MatrixBase<Derived> & F) const
140  {
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>
158  {
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;
180  }; // struct ScaledConstraint
181 
182  template<typename S1, int O1, typename _Constraint>
183  struct MultiplicationOp<InertiaTpl<S1,O1>, ScaledConstraint<_Constraint> >
184  {
187  typedef typename Constraint::Scalar Scalar;
188 
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  {
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  {
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  {
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>
242  {
243  enum
244  {
247  };
248  typedef typename traits<Joint>::Scalar Scalar;
250 
253 
256 
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 
268 
271  };
272 
273  template<class Joint>
276 
277  template<class Joint>
280 
281  template<class JointData>
282  struct JointDataMimic
283  : public JointDataBase< JointDataMimic<JointData> >
284  {
285  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
286 
289 
291 
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 
300  { *this = other; }
301 
303  const Scalar & scaling)
304  : m_jdata_ref(jdata.derived())
305  , m_scaling(scaling)
306  , S(m_jdata_ref.S,scaling)
307  {}
308 
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 
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  {
399  };
400 
401  template<class JointModel>
402  struct JointModelMimic
403  : public JointModelBase< JointModelMimic<JointModel> >
404  {
405  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
406 
408 
409  PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived);
410 
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 
420  {}
421 
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>
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
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__
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > const Eigen::MatrixBase< TangentVectorType > & v
traits< Joint >::Motion_t Motion_t
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...
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef traits< JointModelMimic >::JointDerived JointDerived
JointModelBase< JointModelMimic > Base
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 NQ
Definition: dpendulum.py:8
int idx_q(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointIdxQVisitor to get the index in the full model configuration space...
Eigen::Matrix< Scalar, Constraint::NV, ForceSet::ColsAtCompileTime, Constraint::Options|Eigen::RowMajor > ReturnType
Definition: joint-mimic.hpp:56
ConfigVector_t m_q_transform
Transform configuration vector.
SE3GroupAction< Constraint >::ReturnType SE3ActionReturnType
Definition: joint-mimic.hpp:70
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...
SizeDepType< NV >::template RowsReturn< D >::ConstType jointRows_impl(const Eigen::MatrixBase< D > &A) const
UDTypeConstRef UDinv_accessor() const
Return type of the Constraint::Transpose * ForceSet operation.
TansformTypeConstRef M_accessor() const
TangentVector_t m_v_transform
Transform velocity vector.
int nq(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointNqVisitor to get the dimension of the joint configuration space...
Return type of the ation of a Motion onto an object of type D.
MultiplicationOp< Eigen::MatrixBase< M6Like >, Constraint >::ReturnType ReturnType
ConstraintForceSetOp< Constraint, ForceSet >::ReturnType OriginalReturnType
Definition: joint-mimic.hpp:54
const std::vector< bool > hasConfigurationLimitInTangent() const
ConfigVector_t & jointConfiguration()
DTypeConstRef Dinv_accessor() const
MotionAlgebraAction< ScaledConstraint, MotionDerived >::ReturnType motionAction(const MotionDense< MotionDerived > &m) const
const Base & base() const
TangentVector_t & jointVelocity()
JointIndex id(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointIdVisitor to get the index of the joint in the kinematic chain...
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef traits< JointDataMimic >::JointDerived JointDerived
CastType< NewScalar, JointModelMimic >::type cast() const
DenseBase matrix_impl() const
traits< Constraint >::ConstMatrixReturnType ConstMatrixReturnType
Definition: joint-mimic.hpp:29
void setIndexes_impl(JointIndex id, int, int)
SizeDepType< NQ >::template SegmentReturn< D >::ConstType jointConfigSelector_impl(const Eigen::MatrixBase< D > &a) const
ScaledConstraint< typename traits< Joint >::Constraint_t > Constraint_t
CastType< NewScalar, JointModel >::type JointModelNewType
EIGEN_DONT_INLINE void calc(JointDataDerived &jdata, const typename Eigen::MatrixBase< ConfigVector > &qs, const typename Eigen::MatrixBase< TangentVector > &vs) const
JointMimic< typename traits< Joint >::JointDerived > JointDerived
PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE typedef traits< Joint >::ConfigVector_t ConfigVector_t
#define PINOCCHIO_EIGEN_CONST_CAST(TYPE, OBJ)
Macro for an automatic const_cast.
ConstraintForceOp< Constraint, ForceDerived >::ReturnType OriginalReturnType
Definition: joint-mimic.hpp:44
std::string shortname() const
const JointData & jdata() const
JointDataMimic(const JointDataMimic &other)
SE3::Scalar Scalar
Definition: conversions.cpp:13
JointModelMimic< JointModelBase > JointModelDerived
static BlockReturn< D >::ConstType block(const Eigen::MatrixBase< D > &mat, typename Eigen::DenseBase< D >::Index row_id, typename Eigen::DenseBase< D >::Index col_id, typename Eigen::DenseBase< D >::Index row_size_block=NV, typename Eigen::DenseBase< D >::Index col_size_block=NV)
MultiplicationOp< Inertia, _Constraint >::ReturnType OriginalReturnType
SizeDepType< NV >::template RowsReturn< D >::Type jointRows_impl(Eigen::MatrixBase< D > &A) const
virtual bool isEqual(const CollisionGeometry &other) const=0
SizeDepType< NV >::template SegmentReturn< D >::Type jointVelocitySelector_impl(Eigen::MatrixBase< D > &a) const
#define PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE
const Scalar & offset() const
static ReturnType run(const Inertia &Y, const Constraint &scaled_constraint)
SizeDepType< NV >::template ColsReturn< D >::ConstType jointCols_impl(const Eigen::MatrixBase< D > &A) const
JointMotion __mult__(const Eigen::MatrixBase< VectorLike > &v) const
Definition: joint-mimic.hpp:97
traits< Joint >::JointModelDerived JointModelBase
const TangentVector_t & jointVelocity() const
ConstraintTypeRef S_accessor()
ScaledConstraint(const ScaledConstraint &other)
Definition: joint-mimic.hpp:84
SizeDepType< NV >::template SegmentReturn< D >::ConstType jointVelocitySelector_impl(const Eigen::MatrixBase< D > &a) const
#define PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(Joint)
SizeDepType< NQ >::template SegmentReturn< D >::Type jointConfigSelector_impl(Eigen::MatrixBase< D > &a) const
ConstraintForceSetOp< ScaledConstraint, Derived >::ReturnType operator*(const Eigen::MatrixBase< Derived > &F) const
[CRBA] MatrixBase operator* (Constraint::Transpose S, ForceSet::Block)
JointMimic< typename traits< Joint >::JointDerived > JointDerived
ScaledConstraint & operator=(const ScaledConstraint &other)
Definition: joint-mimic.hpp:89
ConstraintForceOp< ScaledConstraint, Derived >::ReturnType operator*(const ForceDense< Derived > &f) const
traits< Joint >::JointDataDerived JointDataBase
MotionTypeRef v_accessor()
Eigen::Matrix< Scalar, IdealReturnType::RowsAtCompileTime, IdealReturnType::ColsAtCompileTime, Constraint::Options > ReturnType
Definition: joint-mimic.hpp:47
TransposeConst transpose() const
JointDataMimic(const JointDataBase< JointData > &jdata, const Scalar &scaling)
traits< Constraint >::JointForce JointForce
Definition: joint-mimic.hpp:26
ScaledConstraint(const Constraint &constraint, const Scalar &scaling_factor)
Definition: joint-mimic.hpp:78
std::string shortname() const
Vec3f a
MotionTypeConstRef v_accessor() const
void calc_aba(JointDataDerived &data, const Eigen::MatrixBase< Matrix6Like > &I, const bool update_I) const
JointDataMimic< JointDataBase > JointDataDerived
const ConfigVector_t & jointConfiguration() const
static ReturnType run(const Eigen::MatrixBase< M6Like > &Y, const Constraint &scaled_constraint)
ConstraintTypeConstRef S_accessor() const
static ColsReturn< D >::ConstType middleCols(const Eigen::MatrixBase< D > &mat, typename Eigen::DenseBase< D >::Index start, typename Eigen::DenseBase< D >::Index size=NV)
bool isEqual(const JointDataMimic &other) const
SE3ActionReturnType se3Action(const SE3Tpl< S1, O1 > &m) const
Main pinocchio namespace.
Definition: timings.cpp:28
JointDataMimic & operator=(const JointDataMimic &other)
ScalarMatrixProduct< Scalar, OriginalReturnType >::type IdealReturnType
Definition: joint-mimic.hpp:46
static std::string classname()
traits< Constraint >::JointMotion JointMotion
Definition: joint-mimic.hpp:25
SE3ActionReturnType se3ActionInverse(const SE3Tpl< S1, O1 > &m) const
traits< Constraint >::MatrixReturnType MatrixReturnType
Definition: joint-mimic.hpp:28
traits< Joint >::TangentVector_t TangentVector_t
PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorType) integrate(const ModelTpl< Scalar
Integrate a configuration vector for the specified model for a tangent vector during one unit time...
res
void setIndexes(JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel, JointIndex id, int q, int v)
Visit a JointModelTpl through JointSetIndexesVisitor to set the indexes of the joint in the kinematic...
ScaledConstraint(const Scalar &scaling_factor)
Definition: joint-mimic.hpp:74
EIGEN_DONT_INLINE void calc(JointDataDerived &jdata, const typename Eigen::MatrixBase< ConfigVector > &qs) const
bool isEqual(const ScaledConstraint &other) const
UTypeConstRef U_accessor() const
Common traits structure to fully define base classes for CRTP.
Definition: src/fwd.hpp:44
ScalarMatrixProduct< Scalar, OriginalReturnType >::type IdealReturnType
Definition: joint-mimic.hpp:55
traits< Joint >::Transformation_t Transformation_t
const Eigen::CwiseUnaryOp< Eigen::internal::scalar_multiple_op< Scalar >, const Matrix > type
Definition: math/matrix.hpp:82
#define PINOCCHIO_CONSTRAINT_TYPEDEF_TPL(DERIVED)
static std::string classname()
MotionAlgebraAction< Constraint, MotionDerived >::ReturnType ReturnType
Definition: joint-mimic.hpp:38
const JointModel & jmodel() const
static RowsReturn< D >::ConstType middleRows(const Eigen::MatrixBase< D > &mat, typename Eigen::DenseBase< D >::Index start, typename Eigen::DenseBase< D >::Index size=NV)
SizeDepType< NV >::template ColsReturn< D >::Type jointCols_impl(Eigen::MatrixBase< D > &A) const
JointModelMimic(const JointModelBase< JointModel > &jmodel, const Scalar &scaling, const Scalar &offset)
const Scalar & scaling() const
NV
Definition: dcrba.py:444
static SegmentReturn< D >::ConstType segment(const Eigen::MatrixBase< D > &mat, typename Eigen::DenseBase< D >::Index start, typename Eigen::DenseBase< D >::Index size=NV)
Type of the cast of a class C templated by Scalar and Options, to a new NewScalar type...
Definition: src/fwd.hpp:55
JointDataBase< JointDataMimic< JointData > > Base
TansformTypeRef M_accessor()
const Constraint & constraint() const
Return type of the Constraint::Transpose * Force operation.
const std::vector< bool > hasConfigurationLimit() const
TransposeConst(const ScaledConstraint &ref)
JointDataDerived createData() const
BiasTypeConstRef c_accessor() const
ReturnTypeNotDefined ReturnType
const Scalar & scaling() const
SizeDepType< NV >::template BlockReturn< D >::Type jointBlock_impl(Eigen::MatrixBase< D > &Mat) const
ConstraintBase< ScaledConstraint > Base
Definition: joint-mimic.hpp:67
const Scalar & scaling() const
SE3GroupAction< Constraint >::ReturnType ReturnType
Definition: joint-mimic.hpp:34
#define PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(Joint)


pinocchio
Author(s):
autogenerated on Fri Jun 23 2023 02:38:30