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 
10 #include "pinocchio/macros.hpp"
13 
14 namespace pinocchio
15 {
16  template<typename _Scalar, int _Options, int MaxDim>
18 
19  template<typename _Scalar, int _Options, int _MaxDim>
20  struct traits<ScaledJointMotionSubspaceTpl<_Scalar, _Options, _MaxDim>>
21  {
22  enum
23  {
24  MaxDim = _MaxDim
25  };
29  enum
30  {
32  };
33  enum
34  {
37  };
43  }; // traits ScaledJointMotionSubspaceTpl
44 
45  template<typename _Scalar, int _Options, int _MaxDim>
46  struct SE3GroupAction<ScaledJointMotionSubspaceTpl<_Scalar, _Options, _MaxDim>>
47  {
48  typedef typename SE3GroupAction<typename traits<
51  };
52 
53  template<typename _Scalar, int _Options, int _MaxDim, typename MotionDerived>
55  ScaledJointMotionSubspaceTpl<_Scalar, _Options, _MaxDim>,
56  MotionDerived>
57  {
58  typedef typename MotionAlgebraAction<
59  typename traits<
61  MotionDerived>::ReturnType ReturnType;
62  };
63 
64  template<typename _Scalar, int _Options, int _MaxDim, typename ForceDerived>
65  struct ConstraintForceOp<ScaledJointMotionSubspaceTpl<_Scalar, _Options, _MaxDim>, ForceDerived>
66  {
67  typedef
70  typedef
73  };
74 
75  template<typename _Scalar, int _Options, int _MaxDim, typename ForceSet>
76  struct ConstraintForceSetOp<ScaledJointMotionSubspaceTpl<_Scalar, _Options, _MaxDim>, ForceSet>
77  {
78  typedef
81  typedef
84  };
85 
86  template<typename _Scalar, int _Options, int _MaxDim>
88  : JointMotionSubspaceBase<ScaledJointMotionSubspaceTpl<_Scalar, _Options, _MaxDim>>
89  {
90  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
91 
93  enum
94  {
96  MaxDim = _MaxDim
97  };
99  using Base::nv;
100 
102  RefJointMotionSubspace RefJointMotionSubspace;
104 
107  {
108  }
109 
110  explicit ScaledJointMotionSubspaceTpl(const Scalar & scaling_factor)
111  : m_constraint(0)
112  , m_scaling_factor(scaling_factor)
113  {
114  }
115 
116  template<typename ConstraintTpl>
117  ScaledJointMotionSubspaceTpl(const ConstraintTpl & constraint, const Scalar & scaling_factor)
119  , m_scaling_factor(scaling_factor)
120  {
121  }
122 
124  : m_constraint(other.m_constraint)
126  {
127  }
128 
130  {
131  m_constraint = other.m_constraint;
133  return *this;
134  }
135 
136  template<typename VectorLike>
137  JointMotion __mult__(const Eigen::MatrixBase<VectorLike> & v) const
138  {
139 
140  assert(v.size() == nv());
141  JointMotion jm = m_constraint * v;
142  return m_scaling_factor * jm;
143  }
144 
145  template<typename S1, int O1>
147  {
148  return m_scaling_factor * m_constraint.se3Action(m);
149  }
150 
151  template<typename S1, int O1>
153  {
154  return m_scaling_factor * m_constraint.se3ActionInverse(m);
155  }
156 
157  int nv_impl() const
158  {
159  return m_constraint.nv();
160  }
161 
163  {
166  : ref(ref)
167  {
168  }
169 
170  template<typename Derived>
171  // typename ConstraintForceOp<ScaledJointMotionSubspaceTpl, Derived>::ReturnType
172  JointForce operator*(const ForceDense<Derived> & f) const
173  {
174  return ref.m_scaling_factor * (ref.m_constraint.transpose() * f);
175  }
176 
178  template<typename Derived>
180  operator*(const Eigen::MatrixBase<Derived> & F) const
181  {
182  return ref.m_scaling_factor * (ref.m_constraint.transpose() * F);
183  }
184 
185  }; // struct TransposeConst
186 
188  {
189  return TransposeConst(*this);
190  }
191 
192  const DenseBase & matrix_impl() const
193  {
194  S = m_scaling_factor * m_constraint.matrix_impl();
195  return S;
196  }
197 
198  DenseBase & matrix_impl()
199  {
200  S = m_scaling_factor * m_constraint.matrix_impl();
201  return S;
202  }
203 
204  template<typename MotionDerived>
207  {
208  return m_scaling_factor * m_constraint.motionAction(m);
209  }
210 
211  inline const Scalar & scaling() const
212  {
213  return m_scaling_factor;
214  }
215  inline Scalar & scaling()
216  {
217  return m_scaling_factor;
218  }
219 
220  inline const RefJointMotionSubspace & constraint() const
221  {
222  return m_constraint.derived();
223  }
225  {
226  return m_constraint.derived();
227  }
228 
229  bool isEqual(const ScaledJointMotionSubspaceTpl & other) const
230  {
231  return m_constraint == other.m_constraint && m_scaling_factor == other.m_scaling_factor;
232  }
233 
234  protected:
237  mutable DenseBase S;
238  }; // struct ScaledJointMotionSubspaceTpl
239 
240  template<typename S1, int O1, typename S2, int O2, int MD2>
242  {
245  typedef typename Constraint::Scalar Scalar;
246 
247  typedef Eigen::Matrix<S2, 6, Eigen::Dynamic, O2, 6, MD2> ReturnType;
248  };
249 
250  /* [CRBA] ForceSet operator* (Inertia Y,Constraint S) */
251  namespace impl
252  {
253  template<typename S1, int O1, typename S2, int O2, int MD2>
255  {
259 
260  static inline ReturnType run(const Inertia & Y, const Constraint & scaled_constraint)
261  {
262  return scaled_constraint.scaling() * (Y * scaled_constraint.constraint());
263  }
264  };
265  } // namespace impl
266 
267  template<typename M6Like, typename S2, int O2, int MD2>
268  struct MultiplicationOp<Eigen::MatrixBase<M6Like>, ScaledJointMotionSubspaceTpl<S2, O2, MD2>>
269  {
271  typedef Eigen::Matrix<S2, 6, Eigen::Dynamic, O2, 6, MD2> ReturnType;
272  };
273 
274  /* [ABA] operator* (Inertia Y,Constraint S) */
275  namespace impl
276  {
277  template<typename M6Like, typename S2, int O2, int MD2>
278  struct LhsMultiplicationOp<Eigen::MatrixBase<M6Like>, ScaledJointMotionSubspaceTpl<S2, O2, MD2>>
279  {
281  typedef
283 
284  static inline ReturnType
285  run(const Eigen::MatrixBase<M6Like> & Y, const Constraint & scaled_constraint)
286  {
287  return scaled_constraint.scaling() * (Y.derived() * scaled_constraint.constraint());
288  }
289  };
290  } // namespace impl
291 
292  template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
294  template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
295  struct JointModelMimicTpl;
296  template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
297  struct JointDataMimicTpl;
298 
299  template<typename _Scalar, int _Options, template<typename S, int O> class JointCollectionTpl>
300  struct traits<JointMimicTpl<_Scalar, _Options, JointCollectionTpl>>
301  {
302  typedef _Scalar Scalar;
303 
304  enum
305  {
306  Options = _Options,
309  NVExtended = Eigen::Dynamic,
310  MaxNVMimicked = 6
311  };
312 
313  typedef JointCollectionTpl<Scalar, Options> JointCollection;
316 
321 
322  // [ABA]
323  typedef Eigen::Matrix<Scalar, 6, Eigen::Dynamic, Options> U_t;
324  typedef Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, Options> D_t;
325  typedef Eigen::Matrix<Scalar, 6, Eigen::Dynamic, Options> UD_t;
326 
336  typedef U_t UTypeRef;
338  typedef D_t DTypeRef;
340  typedef UD_t UDTypeRef;
341 
342  typedef Eigen::Matrix<Scalar, Eigen::Dynamic, 1, Options> ConfigVector_t;
343  typedef Eigen::Matrix<Scalar, Eigen::Dynamic, 1, Options> TangentVector_t;
344 
349 
350  typedef boost::mpl::false_ is_mimicable_t;
351  };
352 
353  template<typename _Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
354  struct traits<JointDataMimicTpl<_Scalar, Options, JointCollectionTpl>>
355  {
357  typedef _Scalar Scalar;
358  };
359 
360  template<typename _Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
361  struct traits<JointModelMimicTpl<_Scalar, Options, JointCollectionTpl>>
362  {
364  typedef _Scalar Scalar;
365  };
366 
367  template<typename _Scalar, int _Options, template<typename S, int O> class JointCollectionTpl>
368  struct JointDataMimicTpl
369  : public JointDataBase<JointDataMimicTpl<_Scalar, _Options, JointCollectionTpl>>
370  {
371  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
372 
376 
379 
381  : S((Scalar)0)
382  {
383  joint_q.resize(0, 1);
384  joint_q_transformed.resize(0, 1);
385  joint_v.resize(0, 1);
386  joint_v_transformed.resize(0, 1);
387  }
388 
390  const RefJointData & jdata, const Scalar & scaling, const int & nq, const int & nv)
391  : m_jdata_mimicking(checkMimic(jdata.derived()))
392  , S(m_jdata_mimicking.S(), scaling)
393  {
394  joint_q.resize(nq, 1);
395  joint_q_transformed.resize(nq, 1);
396  joint_v.resize(nv, 1);
397  joint_v_transformed.resize(nv, 1);
398  }
399 
401  {
403  joint_q = other.joint_q;
405  joint_v = other.joint_v;
407  S = Constraint_t(other.S);
408  return *this;
409  }
410 
411  using Base::isEqual;
412  bool isEqual(const JointDataMimicTpl & other) const
413  {
414  return Base::isEqual(other) && m_jdata_mimicking == other.m_jdata_mimicking
416  && joint_v == other.joint_v && joint_v_transformed == other.joint_v_transformed;
417  }
418 
419  static std::string classname()
420  {
421  return std::string("JointDataMimic");
422  }
423 
424  std::string shortname() const
425  {
426  return classname();
427  }
428 
429  // // Accessors
430  ConstraintTypeConstRef S_accessor() const
431  {
432  return S;
433  }
434  ConstraintTypeRef S_accessor()
435  {
436  return S;
437  }
438 
439  Transformation_t M_accessor() const
440  {
441  return m_jdata_mimicking.M();
442  }
443 
444  Motion_t v_accessor() const
445  {
446  return m_jdata_mimicking.v();
447  }
448 
449  Bias_t c_accessor() const
450  {
451  return m_jdata_mimicking.c();
452  }
453 
454  U_t U_accessor() const
455  {
456  return m_jdata_mimicking.U();
457  }
458 
459  D_t Dinv_accessor() const
460  {
461  return m_jdata_mimicking.Dinv();
462  }
463 
464  UD_t UDinv_accessor() const
465  {
466  return m_jdata_mimicking.UDinv();
467  }
468 
469  D_t StU_accessor() const
470  {
471  return m_jdata_mimicking.StU();
472  }
473 
474  friend struct JointModelMimicTpl<_Scalar, _Options, JointCollectionTpl>;
475 
476  const RefJointData & jdata() const
477  {
478  return m_jdata_mimicking;
479  }
481  {
482  return m_jdata_mimicking;
483  }
484 
485  ConfigVectorTypeRef joint_q_accessor()
486  {
487  return joint_q;
488  }
489  ConfigVectorTypeConstRef joint_q_accessor() const
490  {
491  return joint_q;
492  }
493 
494  ConfigVector_t & q_transformed()
495  {
496  return joint_q_transformed;
497  }
498  const ConfigVector_t & q_transformed() const
499  {
500  return joint_q_transformed;
501  }
502  TangentVectorTypeRef joint_v_accessor()
503  {
504  return joint_v;
505  }
506  TangentVectorTypeConstRef joint_v_accessor() const
507  {
508  return joint_v;
509  }
510 
511  TangentVector_t & v_transformed()
512  {
513  return joint_v_transformed;
514  }
515  const TangentVector_t & v_transformed() const
516  {
517  return joint_v_transformed;
518  }
519 
520  void disp(std::ostream & os) const
521  {
522  Base::disp(os);
523  os << " Mimicking joint data: " << m_jdata_mimicking.shortname() << std::endl;
524  }
525 
527 
529  ConfigVector_t joint_q;
531  ConfigVector_t joint_q_transformed;
533  TangentVector_t joint_v;
535  TangentVector_t joint_v_transformed;
536  // data
537  Constraint_t S;
538  }; // struct JointDataMimicTpl
539 
540  template<
541  typename NewScalar,
542  typename Scalar,
543  int Options,
544  template<typename S, int O> class JointCollectionTpl>
545  struct CastType<NewScalar, JointModelMimicTpl<Scalar, Options, JointCollectionTpl>>
546  {
548  };
549 
550  template<typename _Scalar, int _Options, template<typename S, int O> class JointCollectionTpl>
551  struct JointModelMimicTpl
552  : public JointModelBase<JointModelMimicTpl<_Scalar, _Options, JointCollectionTpl>>
553  {
554  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
555 
559  enum
560  {
562  };
563 
564  typedef JointCollectionTpl<Scalar, Options> JointCollection;
566 
570 
571  using Base::id;
572  using Base::idx_q;
573  using Base::idx_v;
574  using Base::idx_vExtended;
575  using Base::nq;
576  using Base::nv;
577  using Base::nvExtended;
578  using Base::setIndexes;
579 
581  {
582  }
583 
584  template<typename JointModel>
586  const JointModelBase<JointModel> & jmodel, const Scalar & scaling, const Scalar & offset)
588  {
589  }
590 
591  template<typename JointModelMimicking, typename JointModelMimicked>
593  const JointModelBase<JointModelMimicking> & jmodel_mimicking,
594  const JointModelBase<JointModelMimicked> & jmodel_mimicked,
595  const Scalar & scaling,
596  const Scalar & offset)
597  : m_jmodel_mimicking(checkMimic((JointModel)jmodel_mimicking.derived()))
598  , m_scaling(scaling)
599  , m_offset(offset)
600  , m_nqExtended(jmodel_mimicking.nq())
601  , m_nvExtended(jmodel_mimicking.nvExtended())
602  {
603  assert(jmodel_mimicking.nq() == jmodel_mimicked.nq());
604  assert(jmodel_mimicking.nv() == jmodel_mimicked.nv());
605  assert(jmodel_mimicking.nvExtended() == jmodel_mimicked.nvExtended());
606 
608  jmodel_mimicked.id(), jmodel_mimicked.idx_q(), jmodel_mimicked.idx_v(),
609  jmodel_mimicked.idx_vExtended());
610  }
611 
612  Base & base()
613  {
614  return *static_cast<Base *>(this);
615  }
616  const Base & base() const
617  {
618  return *static_cast<const Base *>(this);
619  }
620 
621  inline int nq_impl() const
622  {
623  return 0;
624  }
625  inline int nv_impl() const
626  {
627  return 0;
628  }
629  inline int nvExtended_impl() const
630  {
631  return m_nvExtended;
632  }
633 
639  void setIndexes_impl(JointIndex id, int /*q*/, int /*v*/, int vExtended)
640  {
642  (id > m_jmodel_mimicking.id()), std::invalid_argument,
643  "Mimic joint index is lower than its directing joint. Should never happen");
644  Base::i_id = id;
645  // Base::i_q = q;
646  // Base::i_v = v;
647  Base::i_vExtended = vExtended;
648  }
649 
659  void setMimicIndexes(JointIndex id, int q, int v, int vExtended)
660  {
661  // Set idx_q, idx_v to zero because only the corresponding subsegment of q,v are passed to the
662  // m_jmodel_mimicking, thus, its indexes starts at 0
663  m_jmodel_mimicking.setIndexes(id, 0, 0, vExtended);
664 
665  // idx_q, idx_v are kept separately to extract the subsegment
666  Base::i_q = q;
667  Base::i_v = v;
668  }
669 
670  JointDataDerived createData() const
671  {
672  return JointDataDerived(
674  }
675 
676  const std::vector<bool> hasConfigurationLimit() const
677  {
679  }
680 
681  const std::vector<bool> hasConfigurationLimitInTangent() const
682  {
684  }
685 
686  template<typename ConfigVector>
687  EIGEN_DONT_INLINE void
688  calc(JointDataDerived & jdata, const typename Eigen::MatrixBase<ConfigVector> & qs) const
689  {
690  jdata.joint_q = qs.segment(Base::i_q, m_nqExtended);
692  m_jmodel_mimicking, jdata.joint_q, m_scaling, m_offset, jdata.joint_q_transformed);
693  m_jmodel_mimicking.calc(jdata.m_jdata_mimicking, jdata.joint_q_transformed);
694  }
695 
696  template<typename ConfigVector, typename TangentVector>
697  EIGEN_DONT_INLINE void calc(
698  JointDataDerived & jdata,
699  const typename Eigen::MatrixBase<ConfigVector> & qs,
700  const typename Eigen::MatrixBase<TangentVector> & vs) const
701  {
702  jdata.joint_q = qs.segment(Base::i_q, m_nqExtended);
703  jdata.joint_v = vs.segment(Base::i_v, m_nvExtended);
705  m_jmodel_mimicking, jdata.joint_q, m_scaling, m_offset, jdata.joint_q_transformed);
706  jdata.joint_v_transformed = m_scaling * jdata.joint_v;
707 
709  jdata.m_jdata_mimicking, jdata.joint_q_transformed, jdata.joint_v_transformed);
710  }
711 
712  template<typename VectorLike, typename Matrix6Like>
713  void calc_aba(
714  JointDataDerived &,
715  const Eigen::MatrixBase<VectorLike> &,
716  const Eigen::MatrixBase<Matrix6Like> &,
717  const bool) const
718  {
719  assert(
720  false
721  && "Joint Mimic is not supported for aba yet. Remove it from your model if you want to use "
722  "this function");
723  }
724 
725  static std::string classname()
726  {
727  return std::string("JointModelMimic");
728  }
729 
730  std::string shortname() const
731  {
732  return classname();
733  }
734 
736  template<typename NewScalar>
738  {
739  typedef typename CastType<NewScalar, JointModelMimicTpl>::type ReturnType;
740  ReturnType res(
741  m_jmodel_mimicking.template cast<NewScalar>(),
744  res.setIndexes(id(), Base::i_q, Base::i_v, Base::i_vExtended);
745  res.setMimicIndexes(m_jmodel_mimicking.id(), Base::i_q, Base::i_v, Base::i_vExtended);
746  return res;
747  }
748 
749  const JointModel & jmodel() const
750  {
751  return m_jmodel_mimicking;
752  }
754  {
755  return m_jmodel_mimicking;
756  }
757 
758  const Scalar & scaling() const
759  {
760  return m_scaling;
761  }
763  {
764  return m_scaling;
765  }
766 
767  const Scalar & offset() const
768  {
769  return m_offset;
770  }
772  {
773  return m_offset;
774  }
775 
776  protected:
777  // data
781 
782  public:
783  /* Acces to dedicated segment in robot config space. */
784  // Const access
785  template<typename D>
786  typename SizeDepType<NQ>::template SegmentReturn<D>::ConstType
787  JointMappedConfigSelector_impl(const Eigen::MatrixBase<D> & a) const
788  {
789  return SizeDepType<NQ>::segment(a.derived(), Base::i_q, m_nqExtended);
790  }
791 
792  // Non-const access
793  template<typename D>
794  typename SizeDepType<NQ>::template SegmentReturn<D>::Type
795  JointMappedConfigSelector_impl(Eigen::MatrixBase<D> & a) const
796  {
797  return SizeDepType<NQ>::segment(a.derived(), Base::i_q, m_nqExtended);
798  }
799  /* Acces to dedicated segment in robot tangent space. */
800  // Const access
801  template<typename D>
802  typename SizeDepType<NQ>::template SegmentReturn<D>::ConstType
803  JointMappedVelocitySelector_impl(const Eigen::MatrixBase<D> & a) const
804  {
805  return SizeDepType<NQ>::segment(a.derived(), Base::i_v, m_nvExtended);
806  }
807 
808  // Non-const access
809  template<typename D>
810  typename SizeDepType<NQ>::template SegmentReturn<D>::Type
811  JointMappedVelocitySelector_impl(Eigen::MatrixBase<D> & a) const
812  {
813  return SizeDepType<NQ>::segment(a.derived(), Base::i_v, m_nvExtended);
814  }
815 
816  /* Acces to dedicated columns in a ForceSet or MotionSet matrix.*/
817  // Const access
818  template<typename D>
819  typename SizeDepType<NV>::template ColsReturn<D>::ConstType
820  jointCols_impl(const Eigen::MatrixBase<D> & A) const
821  {
822  return SizeDepType<NV>::middleCols(A.derived(), Base::i_v, m_nvExtended);
823  }
824 
825  // Non-const access
826  template<typename D>
827  typename SizeDepType<NV>::template ColsReturn<D>::Type
828  jointCols_impl(Eigen::MatrixBase<D> & A) const
829  {
830  return SizeDepType<NV>::middleCols(A.derived(), Base::i_v, m_nvExtended);
831  }
832 
833  /* Acces to dedicated rows in a matrix.*/
834  // Const access
835  template<typename D>
836  typename SizeDepType<NV>::template RowsReturn<D>::ConstType
837  jointRows_impl(const Eigen::MatrixBase<D> & A) const
838  {
839  return SizeDepType<NV>::middleRows(A.derived(), Base::i_v, m_nvExtended);
840  }
841 
842  // Non-const access
843  template<typename D>
844  typename SizeDepType<NV>::template RowsReturn<D>::Type
845  jointRows_impl(Eigen::MatrixBase<D> & A) const
846  {
847  return SizeDepType<NV>::middleRows(A.derived(), Base::i_v, m_nvExtended);
848  }
849 
850  // /// \brief Returns a block of dimension nv()xnv() located at position idx_v(),idx_v() in the
851  // matrix Mat
852  // // Const access
853  template<typename D>
854  typename SizeDepType<NV>::template BlockReturn<D>::ConstType
855  jointBlock_impl(const Eigen::MatrixBase<D> & Mat) const
856  {
857  return SizeDepType<NV>::block(
858  Mat.derived(), Base::i_v, Base::i_v, m_nvExtended, m_nvExtended);
859  }
860 
861  // Non-const access
862  template<typename D>
863  typename SizeDepType<NV>::template BlockReturn<D>::Type
864  jointBlock_impl(Eigen::MatrixBase<D> & Mat) const
865  {
866  return SizeDepType<NV>::block(
867  Mat.derived(), Base::i_v, Base::i_v, m_nvExtended, m_nvExtended);
868  }
869 
870  void disp(std::ostream & os) const
871  {
872  Base::disp(os);
873  os << " Mimicking joint type: " << m_jmodel_mimicking.shortname() << std::endl;
874  os << " Mimicked joint id: " << m_jmodel_mimicking.id() << std::endl;
875  os << " Mimic scaling: " << m_scaling << std::endl;
876  os << " Mimic offset: " << m_offset << std::endl;
877  }
878 
879  }; // struct JointModelMimicTpl
880 
881 } // namespace pinocchio
882 
883 #include <boost/type_traits.hpp>
884 
885 namespace boost
886 {
887  template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
888  struct has_nothrow_constructor<
889  ::pinocchio::JointModelMimicTpl<Scalar, Options, JointCollectionTpl>>
890  : public integral_constant<bool, true>
891  {
892  };
893 
894  template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
895  struct has_nothrow_copy<::pinocchio::JointModelMimicTpl<Scalar, Options, JointCollectionTpl>>
896  : public integral_constant<bool, true>
897  {
898  };
899 
900  template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
901  struct has_nothrow_constructor<
902  ::pinocchio::JointDataMimicTpl<Scalar, Options, JointCollectionTpl>>
903  : public integral_constant<bool, true>
904  {
905  };
906 
907  template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
908  struct has_nothrow_copy<::pinocchio::JointDataMimicTpl<Scalar, Options, JointCollectionTpl>>
909  : public integral_constant<bool, true>
910  {
911  };
912 } // namespace boost
913 
914 #endif // ifndef __pinocchio_multibody_joint_mimic_hpp__
pinocchio::ScaledJointMotionSubspaceTpl::motionAction
MotionAlgebraAction< ScaledJointMotionSubspaceTpl, MotionDerived >::ReturnType motionAction(const MotionDense< MotionDerived > &m) const
Definition: joint-mimic.hpp:206
pinocchio::InertiaTpl
Definition: context/generic.hpp:33
pinocchio::SizeDepType
Definition: matrix-block.hpp:13
pinocchio::JointDataMimicTpl::joint_q_accessor
ConfigVectorTypeConstRef joint_q_accessor() const
Definition: joint-mimic.hpp:489
pinocchio::JointDataMimicTpl::UDinv_accessor
UD_t UDinv_accessor() const
Definition: joint-mimic.hpp:464
pinocchio::JointDataTpl::v
Motion_t v() const
Definition: joint-generic.hpp:134
pinocchio::JointMotionSubspaceBase::nv
int nv() const
Definition: joint-motion-subspace-base.hpp:91
pinocchio::ScaledJointMotionSubspaceTpl::TransposeConst::operator*
ConstraintForceSetOp< ScaledJointMotionSubspaceTpl, Derived >::ReturnType operator*(const Eigen::MatrixBase< Derived > &F) const
[CRBA] MatrixBase operator* (RefConstraint::Transpose S, ForceSet::Block)
Definition: joint-mimic.hpp:180
Eigen
pinocchio::MultiplicationOp
Forward declaration of the multiplication operation return type. Should be overloaded,...
Definition: binary-op.hpp:15
pinocchio::ScaledJointMotionSubspaceTpl::matrix_impl
const DenseBase & matrix_impl() const
Definition: joint-mimic.hpp:192
test-cpp2pybind11.m
m
Definition: test-cpp2pybind11.py:25
pinocchio::traits< JointMimicTpl< _Scalar, _Options, JointCollectionTpl > >::Motion_t
MotionTpl< Scalar, Options > Motion_t
Definition: joint-mimic.hpp:319
pinocchio::JointModelMimicTpl::Motion
MotionTpl< Scalar, Options > Motion
Definition: joint-mimic.hpp:568
pinocchio::ScaledJointMotionSubspaceTpl::S
DenseBase S
Definition: joint-mimic.hpp:237
pinocchio::ScaledJointMotionSubspaceTpl::NV
@ NV
Definition: joint-mimic.hpp:95
pinocchio::JointModelMimicTpl::JointDerived
JointMimicTpl< _Scalar, _Options, JointCollectionTpl > JointDerived
Definition: joint-mimic.hpp:557
pinocchio::JointModelTpl::hasConfigurationLimitInTangent
const std::vector< bool > hasConfigurationLimitInTangent() const
Definition: joint-generic.hpp:297
pinocchio::JointModelMimicTpl::Inertia
InertiaTpl< Scalar, Options > Inertia
Definition: joint-mimic.hpp:569
pinocchio::JointModelMimicTpl::cast
CastType< NewScalar, JointModelMimicTpl >::type cast() const
Definition: joint-mimic.hpp:737
pinocchio::JointModelMimicTpl::jmodel
JointModel & jmodel()
Definition: joint-mimic.hpp:753
pinocchio::ScaledJointMotionSubspaceTpl::operator=
ScaledJointMotionSubspaceTpl & operator=(const ScaledJointMotionSubspaceTpl &other)
Definition: joint-mimic.hpp:129
pinocchio::JointModelMimicTpl::JointMappedVelocitySelector_impl
SizeDepType< NQ >::template SegmentReturn< D >::Type JointMappedVelocitySelector_impl(Eigen::MatrixBase< D > &a) const
Definition: joint-mimic.hpp:811
pinocchio::ScaledJointMotionSubspaceTpl
Definition: joint-mimic.hpp:17
pinocchio::JointModelMimicTpl::hasConfigurationLimitInTangent
const std::vector< bool > hasConfigurationLimitInTangent() const
Definition: joint-mimic.hpp:681
macros.hpp
PINOCCHIO_THROW
#define PINOCCHIO_THROW(condition, exception_type, message)
Generic macro to throw an exception in Pinocchio if the condition is not met with a given input messa...
Definition: include/pinocchio/macros.hpp:158
pinocchio::impl::LhsMultiplicationOp< InertiaTpl< S1, O1 >, ScaledJointMotionSubspaceTpl< S2, O2, MD2 > >::Inertia
InertiaTpl< S1, O1 > Inertia
Definition: joint-mimic.hpp:256
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::JointDataMimicTpl::q_transformed
const ConfigVector_t & q_transformed() const
Definition: joint-mimic.hpp:498
pinocchio::JointDataMimicTpl::disp
void disp(std::ostream &os) const
Definition: joint-mimic.hpp:520
pinocchio::JointModelMimicTpl::jointRows_impl
SizeDepType< NV >::template RowsReturn< D >::ConstType jointRows_impl(const Eigen::MatrixBase< D > &A) const
Definition: joint-mimic.hpp:837
pinocchio::JointModelBase
Definition: joint-model-base.hpp:78
pinocchio::CastType< NewScalar, JointModelMimicTpl< Scalar, Options, JointCollectionTpl > >::type
JointModelMimicTpl< NewScalar, Options, JointCollectionTpl > type
Definition: joint-mimic.hpp:547
pinocchio::JointDataMimicTpl::m_jdata_mimicking
RefJointData m_jdata_mimicking
Definition: joint-mimic.hpp:526
pinocchio::ConstraintForceOp
Return type of the Constraint::Transpose * Force operation.
Definition: joint-motion-subspace-base.hpp:46
pinocchio::idx_v
int idx_v(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointIdxVVisitor to get the index in the model tangent space correspond...
pinocchio::traits< JointMimicTpl< _Scalar, _Options, JointCollectionTpl > >::TangentVector_t
Eigen::Matrix< Scalar, Eigen::Dynamic, 1, Options > TangentVector_t
Definition: joint-mimic.hpp:343
pinocchio::Options
Options
Definition: joint-configuration.hpp:1082
pinocchio::SizeDepType::middleCols
static ColsReturn< D >::ConstType middleCols(const Eigen::MatrixBase< D > &mat, typename Eigen::DenseBase< D >::Index start, typename Eigen::DenseBase< D >::Index size=NV)
Definition: matrix-block.hpp:50
pinocchio::traits< JointMimicTpl< _Scalar, _Options, JointCollectionTpl > >::UDTypeConstRef
UD_t UDTypeConstRef
Definition: joint-mimic.hpp:339
pinocchio::ScaledJointMotionSubspaceTpl::RefJointMotionSubspace
traits< ScaledJointMotionSubspaceTpl< _Scalar, _Options, _MaxDim > >::RefJointMotionSubspace RefJointMotionSubspace
Definition: joint-mimic.hpp:102
pinocchio::JointDataMimicTpl::joint_v_transformed
TangentVector_t joint_v_transformed
Transform velocity vector.
Definition: joint-mimic.hpp:535
pinocchio::traits< JointModelMimicTpl< _Scalar, Options, JointCollectionTpl > >::JointDerived
JointMimicTpl< _Scalar, Options, JointCollectionTpl > JointDerived
Definition: joint-mimic.hpp:363
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::JointMotionSubspaceBase
Definition: joint-motion-subspace-base.hpp:59
pinocchio::JointDataTpl::StU
D_t StU() const
Definition: joint-generic.hpp:156
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::JointDataMimicTpl::RefJointDataVariant
RefJointData::JointDataVariant RefJointDataVariant
Definition: joint-mimic.hpp:378
pinocchio::traits< JointMimicTpl< _Scalar, _Options, JointCollectionTpl > >::MotionTypeConstRef
Motion_t MotionTypeConstRef
Definition: joint-mimic.hpp:331
pinocchio::SE3Tpl
Definition: context/casadi.hpp:30
pinocchio::JointDataMimicTpl::q_transformed
ConfigVector_t & q_transformed()
Definition: joint-mimic.hpp:494
pinocchio::JointModelMimicTpl::offset
const Scalar & offset() const
Definition: joint-mimic.hpp:767
pinocchio::nq
int nq(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointNqVisitor to get the dimension of the joint configuration space.
pinocchio::SE3GroupAction::ReturnType
D ReturnType
Definition: spatial/se3.hpp:41
pinocchio::JointDataBase
Definition: joint-data-base.hpp:161
pinocchio::ScaledJointMotionSubspaceTpl::constraint
RefJointMotionSubspace & constraint()
Definition: joint-mimic.hpp:224
pinocchio::JointDataMimicTpl::operator=
JointDataMimicTpl & operator=(const JointDataMimicTpl &other)
Definition: joint-mimic.hpp:400
pinocchio::ScaledJointMotionSubspaceTpl::TransposeConst::TransposeConst
TransposeConst(const ScaledJointMotionSubspaceTpl &ref)
Definition: joint-mimic.hpp:165
pinocchio::ScaledJointMotionSubspaceTpl::TransposeConst
Definition: joint-mimic.hpp:162
pinocchio::JointDataMimicTpl::isEqual
bool isEqual(const JointDataMimicTpl &other) const
Definition: joint-mimic.hpp:412
pinocchio::traits< JointMimicTpl< _Scalar, _Options, JointCollectionTpl > >::BiasTypeConstRef
Bias_t BiasTypeConstRef
Definition: joint-mimic.hpp:333
pinocchio::ConstraintForceSetOp< ScaledJointMotionSubspaceTpl< _Scalar, _Options, _MaxDim >, ForceSet >::RefReturnType
ConstraintForceSetOp< RefJointMotionSubspace, ForceSet >::ReturnType RefReturnType
Definition: joint-mimic.hpp:82
pinocchio::ConstraintForceOp< ScaledJointMotionSubspaceTpl< _Scalar, _Options, _MaxDim >, ForceDerived >::ReturnType
ScalarMatrixProduct< _Scalar, RefReturnType >::type ReturnType
Definition: joint-mimic.hpp:72
pinocchio::JointModelMimicTpl::setIndexes_impl
void setIndexes_impl(JointIndex id, int, int, int vExtended)
Definition: joint-mimic.hpp:639
pinocchio::MultiplicationOp< InertiaTpl< S1, O1 >, ScaledJointMotionSubspaceTpl< S2, O2, MD2 > >::ReturnType
Eigen::Matrix< S2, 6, Eigen::Dynamic, O2, 6, MD2 > ReturnType
Definition: joint-mimic.hpp:247
pinocchio::MotionDense
Definition: context/casadi.hpp:37
pinocchio::python::Scalar
context::Scalar Scalar
Definition: admm-solver.cpp:29
pinocchio::traits< JointMimicTpl< _Scalar, _Options, JointCollectionTpl > >::UDTypeRef
UD_t UDTypeRef
Definition: joint-mimic.hpp:340
pinocchio::JointDataMimicTpl::shortname
std::string shortname() const
Definition: joint-mimic.hpp:424
pinocchio::traits< JointMimicTpl< _Scalar, _Options, JointCollectionTpl > >::DTypeConstRef
D_t DTypeConstRef
Definition: joint-mimic.hpp:337
pinocchio::JointModelMimicTpl::Base
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef JointModelBase< JointModelMimicTpl > Base
Definition: joint-mimic.hpp:556
pinocchio::traits< JointMimicTpl< _Scalar, _Options, JointCollectionTpl > >::ConfigVector_t
Eigen::Matrix< Scalar, Eigen::Dynamic, 1, Options > ConfigVector_t
Definition: joint-mimic.hpp:342
pinocchio::traits< JointMimicTpl< _Scalar, _Options, JointCollectionTpl > >::JointDataDerived
JointDataMimicTpl< Scalar, Options, JointCollectionTpl > JointDataDerived
Definition: joint-mimic.hpp:314
pinocchio::traits< JointMimicTpl< _Scalar, _Options, JointCollectionTpl > >::Scalar
_Scalar Scalar
Definition: joint-mimic.hpp:302
pinocchio::JointModelMimicTpl::m_offset
Scalar m_offset
Definition: joint-mimic.hpp:779
pinocchio::JointDataMimicTpl::RefJointData
JointDataTpl< _Scalar, _Options, JointCollectionTpl > RefJointData
Definition: joint-mimic.hpp:377
boost
pinocchio::JointModelTpl::hasConfigurationLimit
const std::vector< bool > hasConfigurationLimit() const
Definition: joint-generic.hpp:292
pinocchio::JointDataTpl
Definition: multibody/joint/fwd.hpp:176
pinocchio::ConstraintForceOp::ReturnType
ReturnTypeNotDefined ReturnType
Definition: joint-motion-subspace-base.hpp:48
pinocchio::JointDataMimicTpl::S_accessor
ConstraintTypeConstRef S_accessor() const
Definition: joint-mimic.hpp:430
pinocchio::JointModelMimicTpl::m_nvExtended
int m_nvExtended
Definition: joint-mimic.hpp:780
pinocchio::JointDataMimicTpl::c_accessor
Bias_t c_accessor() const
Definition: joint-mimic.hpp:449
pinocchio::res
ReturnType res
Definition: spatial/classic-acceleration.hpp:57
pinocchio::JointModelMimicTpl::disp
void disp(std::ostream &os) const
Definition: joint-mimic.hpp:870
pinocchio::ScaledJointMotionSubspaceTpl::se3Action
SE3ActionReturnType se3Action(const SE3Tpl< S1, O1 > &m) const
Definition: joint-mimic.hpp:146
pinocchio::traits< JointMimicTpl< _Scalar, _Options, JointCollectionTpl > >::Transformation_t
SE3Tpl< Scalar, Options > Transformation_t
Definition: joint-mimic.hpp:318
pinocchio::JointDataMimicTpl::joint_v
TangentVector_t joint_v
original velocity vector
Definition: joint-mimic.hpp:533
pinocchio::JointModelMimicTpl::nq_impl
int nq_impl() const
Definition: joint-mimic.hpp:621
pinocchio::MotionAlgebraAction
Return type of the ation of a Motion onto an object of type D.
Definition: spatial/motion.hpp:45
pinocchio::ScaledJointMotionSubspaceTpl::scaling
const Scalar & scaling() const
Definition: joint-mimic.hpp:211
pinocchio::JointModelMimicTpl::jointCols_impl
SizeDepType< NV >::template ColsReturn< D >::Type jointCols_impl(Eigen::MatrixBase< D > &A) const
Definition: joint-mimic.hpp:828
pinocchio::traits< ScaledJointMotionSubspaceTpl< _Scalar, _Options, _MaxDim > >::DenseBase
traits< RefJointMotionSubspace >::DenseBase DenseBase
Definition: joint-mimic.hpp:40
pinocchio::MotionAlgebraAction< ScaledJointMotionSubspaceTpl< _Scalar, _Options, _MaxDim >, MotionDerived >::ReturnType
MotionAlgebraAction< typename traits< ScaledJointMotionSubspaceTpl< _Scalar, _Options, _MaxDim > >::RefJointMotionSubspace, MotionDerived >::ReturnType ReturnType
Definition: joint-mimic.hpp:61
pinocchio::impl::LhsMultiplicationOp
Definition: binary-op.hpp:20
autodiff-rnea.f
f
Definition: autodiff-rnea.py:24
pinocchio::JointDataTpl::c
Bias_t c() const
Definition: joint-generic.hpp:138
pinocchio::JointDataTpl::JointDataVariant
JointCollection::JointDataVariant JointDataVariant
Definition: joint-generic.hpp:104
pinocchio::JointModelMimicTpl::JointModel
JointModelTpl< Scalar, Options, JointCollectionTpl > JointModel
Definition: joint-mimic.hpp:565
pinocchio::MultiplicationOp< InertiaTpl< S1, O1 >, ScaledJointMotionSubspaceTpl< S2, O2, MD2 > >::Constraint
ScaledJointMotionSubspaceTpl< S2, O2, MD2 > Constraint
Definition: joint-mimic.hpp:244
pinocchio::JointModelMimicTpl::calc
EIGEN_DONT_INLINE void calc(JointDataDerived &jdata, const typename Eigen::MatrixBase< ConfigVector > &qs, const typename Eigen::MatrixBase< TangentVector > &vs) const
Definition: joint-mimic.hpp:697
pinocchio::traits< JointMimicTpl< _Scalar, _Options, JointCollectionTpl > >::UTypeRef
U_t UTypeRef
Definition: joint-mimic.hpp:336
pinocchio::traits< JointMimicTpl< _Scalar, _Options, JointCollectionTpl > >::UD_t
Eigen::Matrix< Scalar, 6, Eigen::Dynamic, Options > UD_t
Definition: joint-mimic.hpp:325
pinocchio::traits< JointMimicTpl< _Scalar, _Options, JointCollectionTpl > >::TangentVectorTypeRef
TangentVector_t & TangentVectorTypeRef
Definition: joint-mimic.hpp:348
pinocchio::SE3GroupAction
Definition: spatial/se3.hpp:39
pinocchio::JointModelMimicTpl::m_nqExtended
int m_nqExtended
Definition: joint-mimic.hpp:780
pinocchio::JointModelMimicTpl::JointModelMimicTpl
JointModelMimicTpl(const JointModelBase< JointModelMimicking > &jmodel_mimicking, const JointModelBase< JointModelMimicked > &jmodel_mimicked, const Scalar &scaling, const Scalar &offset)
Definition: joint-mimic.hpp:592
pinocchio::JointDataMimicTpl::joint_q_accessor
ConfigVectorTypeRef joint_q_accessor()
Definition: joint-mimic.hpp:485
pinocchio::MotionAlgebraAction::ReturnType
D ReturnType
Definition: spatial/motion.hpp:47
pinocchio::traits< JointMimicTpl< _Scalar, _Options, JointCollectionTpl > >::JointCollection
JointCollectionTpl< Scalar, Options > JointCollection
Definition: joint-mimic.hpp:313
pinocchio::ScaledJointMotionSubspaceTpl::m_scaling_factor
Scalar m_scaling_factor
Definition: joint-mimic.hpp:236
pinocchio::JointDataMimicTpl::v_transformed
TangentVector_t & v_transformed()
Definition: joint-mimic.hpp:511
pinocchio::SE3GroupAction< ScaledJointMotionSubspaceTpl< _Scalar, _Options, _MaxDim > >::ReturnType
SE3GroupAction< typename traits< ScaledJointMotionSubspaceTpl< _Scalar, _Options, _MaxDim > >::RefJointMotionSubspace >::ReturnType ReturnType
Definition: joint-mimic.hpp:50
pinocchio::JointDataMimicTpl::Base
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef JointDataBase< JointDataMimicTpl > Base
Definition: joint-mimic.hpp:373
pinocchio::ScaledJointMotionSubspaceTpl::ScaledJointMotionSubspaceTpl
ScaledJointMotionSubspaceTpl(const ScaledJointMotionSubspaceTpl &other)
Definition: joint-mimic.hpp:123
pinocchio::JointModelTpl::setIndexes
void setIndexes(JointIndex id, int nq, int nv)
Definition: joint-generic.hpp:450
pinocchio::impl::LhsMultiplicationOp< Eigen::MatrixBase< M6Like >, ScaledJointMotionSubspaceTpl< S2, O2, MD2 > >::Constraint
ScaledJointMotionSubspaceTpl< S2, O2, MD2 > Constraint
Definition: joint-mimic.hpp:280
pinocchio::MultiplicationOp< InertiaTpl< S1, O1 >, ScaledJointMotionSubspaceTpl< S2, O2, MD2 > >::Inertia
InertiaTpl< S1, O1 > Inertia
Definition: joint-mimic.hpp:243
pinocchio::JointDataMimicTpl::S
Constraint_t S
Definition: joint-mimic.hpp:537
pinocchio::traits< ScaledJointMotionSubspaceTpl< _Scalar, _Options, _MaxDim > >::RefJointMotionSubspace
JointMotionSubspaceTpl< Eigen::Dynamic, _Scalar, _Options, _MaxDim > RefJointMotionSubspace
Definition: joint-mimic.hpp:27
pinocchio::JointModelMimicTpl::JointModelMimicTpl
JointModelMimicTpl()
Definition: joint-mimic.hpp:580
codegen-rnea.nq
nq
Definition: codegen-rnea.py:18
pinocchio::JointDataMimicTpl::M_accessor
Transformation_t M_accessor() const
Definition: joint-mimic.hpp:439
joint-collection.hpp
pinocchio::ConstraintForceSetOp::ReturnType
ReturnTypeNotDefined ReturnType
Definition: joint-motion-subspace-base.hpp:55
pinocchio::traits< JointMimicTpl< _Scalar, _Options, JointCollectionTpl > >::TangentVectorTypeConstRef
const typedef TangentVector_t TangentVectorTypeConstRef
Definition: joint-mimic.hpp:347
pinocchio::traits< JointMimicTpl< _Scalar, _Options, JointCollectionTpl > >::MotionTypeRef
Motion_t MotionTypeRef
Definition: joint-mimic.hpp:332
pinocchio::JointModelMimicTpl::base
const Base & base() const
Definition: joint-mimic.hpp:616
pinocchio::JointModelMimicTpl::base
Base & base()
Definition: joint-mimic.hpp:612
pinocchio::JointModelBase::nvExtended
int nvExtended() const
Definition: joint-model-base.hpp:152
pinocchio::JointModelMimicTpl::classname
static std::string classname()
Definition: joint-mimic.hpp:725
pinocchio::ScaledJointMotionSubspaceTpl::m_constraint
RefJointMotionSubspace m_constraint
Definition: joint-mimic.hpp:235
pinocchio::traits< JointModelMimicTpl< _Scalar, Options, JointCollectionTpl > >::Scalar
_Scalar Scalar
Definition: joint-mimic.hpp:364
pinocchio::JointDataMimicTpl::joint_v_accessor
TangentVectorTypeConstRef joint_v_accessor() const
Definition: joint-mimic.hpp:506
pinocchio::JointDataMimicTpl::S_accessor
ConstraintTypeRef S_accessor()
Definition: joint-mimic.hpp:434
pinocchio::traits< JointMimicTpl< _Scalar, _Options, JointCollectionTpl > >::is_mimicable_t
boost::mpl::false_ is_mimicable_t
Definition: joint-mimic.hpp:350
pinocchio::traits< JointMimicTpl< _Scalar, _Options, JointCollectionTpl > >::ConstraintTypeRef
Constraint_t & ConstraintTypeRef
Definition: joint-mimic.hpp:328
pinocchio::ScaledJointMotionSubspaceTpl::TransposeConst::operator*
JointForce operator*(const ForceDense< Derived > &f) const
Definition: joint-mimic.hpp:172
pinocchio::JointModelMimicTpl::nvExtended_impl
int nvExtended_impl() const
Definition: joint-mimic.hpp:629
pinocchio::ScaledJointMotionSubspaceTpl::constraint
const RefJointMotionSubspace & constraint() const
Definition: joint-mimic.hpp:220
pinocchio::JointModelTpl::createData
JointDataDerived createData() const
Definition: joint-generic.hpp:320
pinocchio::Dynamic
const int Dynamic
Definition: fwd.hpp:140
pinocchio::configVectorAffineTransform
void configVectorAffineTransform(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel, const Eigen::MatrixBase< ConfigVectorIn > &qIn, const Scalar &scaling, const Scalar &offset, const Eigen::MatrixBase< ConfigVectorOut > &qOut)
Apply the correct affine transform, on a joint configuration, depending on the joint type.
pinocchio::MultiplicationOp< Eigen::MatrixBase< M6Like >, ScaledJointMotionSubspaceTpl< S2, O2, MD2 > >::MotionSubspace
ScaledJointMotionSubspaceTpl< S2, O2, MD2 > MotionSubspace
Definition: joint-mimic.hpp:270
pinocchio::JointModelMimicTpl::m_jmodel_mimicking
JointModel m_jmodel_mimicking
Definition: joint-mimic.hpp:778
pinocchio::JointModelMimicTpl::JointModelMimicTpl
JointModelMimicTpl(const JointModelBase< JointModel > &jmodel, const Scalar &scaling, const Scalar &offset)
Definition: joint-mimic.hpp:585
pinocchio::traits< ScaledJointMotionSubspaceTpl< _Scalar, _Options, _MaxDim > >::ConstMatrixReturnType
traits< RefJointMotionSubspace >::ConstMatrixReturnType ConstMatrixReturnType
Definition: joint-mimic.hpp:42
pinocchio::JointModelMimicTpl::jmodel
const JointModel & jmodel() const
Definition: joint-mimic.hpp:749
pinocchio::JointModelTpl< Scalar, Options, JointCollectionTpl >
pinocchio::JointMimicTpl
Definition: joint-mimic.hpp:293
pinocchio::JointModelMimicTpl::SE3
SE3Tpl< Scalar, Options > SE3
Definition: joint-mimic.hpp:567
pinocchio::JointDataMimicTpl::PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE
PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(JointDerived)
pinocchio::JointDataTpl::U
U_t U() const
Definition: joint-generic.hpp:144
pinocchio::JointDataMimicTpl::joint_q
ConfigVector_t joint_q
original configuration vector
Definition: joint-mimic.hpp:529
joint-base.hpp
pinocchio::traits< JointMimicTpl< _Scalar, _Options, JointCollectionTpl > >::TansformTypeRef
Transformation_t TansformTypeRef
Definition: joint-mimic.hpp:330
pinocchio::traits< JointMimicTpl< _Scalar, _Options, JointCollectionTpl > >::Bias_t
MotionTpl< Scalar, Options > Bias_t
Definition: joint-mimic.hpp:320
pinocchio::SizeDepType::middleRows
static RowsReturn< D >::ConstType middleRows(const Eigen::MatrixBase< D > &mat, typename Eigen::DenseBase< D >::Index start, typename Eigen::DenseBase< D >::Index size=NV)
Definition: matrix-block.hpp:77
pinocchio::impl::LhsMultiplicationOp< InertiaTpl< S1, O1 >, ScaledJointMotionSubspaceTpl< S2, O2, MD2 > >::Constraint
ScaledJointMotionSubspaceTpl< S2, O2, MD2 > Constraint
Definition: joint-mimic.hpp:257
pinocchio::JointModelMimicTpl::JointMappedConfigSelector_impl
SizeDepType< NQ >::template SegmentReturn< D >::Type JointMappedConfigSelector_impl(Eigen::MatrixBase< D > &a) const
Definition: joint-mimic.hpp:795
pinocchio::MultiplicationOp< Eigen::MatrixBase< M6Like >, ScaledJointMotionSubspaceTpl< S2, O2, MD2 > >::ReturnType
Eigen::Matrix< S2, 6, Eigen::Dynamic, O2, 6, MD2 > ReturnType
Definition: joint-mimic.hpp:271
pinocchio::SizeDepType::segment
static SegmentReturn< D >::ConstType segment(const Eigen::MatrixBase< D > &mat, typename Eigen::DenseBase< D >::Index start, typename Eigen::DenseBase< D >::Index size=NV)
Definition: matrix-block.hpp:23
pinocchio::JointModelMimicTpl::jointCols_impl
SizeDepType< NV >::template ColsReturn< D >::ConstType jointCols_impl(const Eigen::MatrixBase< D > &A) const
Definition: joint-mimic.hpp:820
pinocchio::JointModelMimicTpl::PINOCCHIO_JOINT_TYPEDEF_TEMPLATE
PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived)
pinocchio::JointModelBase::idx_vExtended
int idx_vExtended() const
Definition: joint-model-base.hpp:179
simulation-contact-dynamics.A
A
Definition: simulation-contact-dynamics.py:110
pinocchio::traits< ScaledJointMotionSubspaceTpl< _Scalar, _Options, _MaxDim > >::MatrixReturnType
traits< RefJointMotionSubspace >::MatrixReturnType MatrixReturnType
Definition: joint-mimic.hpp:41
pinocchio::traits< JointMimicTpl< _Scalar, _Options, JointCollectionTpl > >::ConfigVectorTypeConstRef
const typedef ConfigVector_t & ConfigVectorTypeConstRef
Definition: joint-mimic.hpp:345
pinocchio::JointModelMimicTpl::MaxNVMimicked
@ MaxNVMimicked
Definition: joint-mimic.hpp:561
pinocchio::impl::LhsMultiplicationOp< Eigen::MatrixBase< M6Like >, ScaledJointMotionSubspaceTpl< S2, O2, MD2 > >::run
static ReturnType run(const Eigen::MatrixBase< M6Like > &Y, const Constraint &scaled_constraint)
Definition: joint-mimic.hpp:285
pinocchio::JointModelMimicTpl::shortname
std::string shortname() const
Definition: joint-mimic.hpp:730
pinocchio::MultiplicationOp< InertiaTpl< S1, O1 >, ScaledJointMotionSubspaceTpl< S2, O2, MD2 > >::Scalar
Constraint::Scalar Scalar
Definition: joint-mimic.hpp:245
pinocchio::JointDataTpl::UDinv
UD_t UDinv() const
Definition: joint-generic.hpp:152
pinocchio::ScaledJointMotionSubspaceTpl::ScaledJointMotionSubspaceTpl
ScaledJointMotionSubspaceTpl(const Scalar &scaling_factor)
Definition: joint-mimic.hpp:110
pinocchio::JointModelBase::nq
int nq() const
Definition: joint-model-base.hpp:148
pinocchio::JointDataMimicTpl::classname
static std::string classname()
Definition: joint-mimic.hpp:419
pinocchio::traits< JointMimicTpl< _Scalar, _Options, JointCollectionTpl > >::ConstraintTypeConstRef
const typedef Constraint_t & ConstraintTypeConstRef
Definition: joint-mimic.hpp:327
pinocchio::traits< ScaledJointMotionSubspaceTpl< _Scalar, _Options, _MaxDim > >::JointForce
traits< RefJointMotionSubspace >::JointForce JointForce
Definition: joint-mimic.hpp:39
pinocchio::JointDataMimicTpl::jdata
RefJointData & jdata()
Definition: joint-mimic.hpp:480
pinocchio::nvExtended
int nvExtended(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointNvExtendVisitor to get the dimension of the joint extended tangent...
pinocchio::ScaledJointMotionSubspaceTpl::ScaledJointMotionSubspaceTpl
ScaledJointMotionSubspaceTpl()
Definition: joint-mimic.hpp:105
pinocchio::q
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > & q
Definition: joint-configuration.hpp:1083
pinocchio::idx_vExtended
int idx_vExtended(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointIdvExtendedVisitor to get the index in the model extended tangent ...
a
Vec3f a
pinocchio::JointModelMimicTpl::JointMappedConfigSelector_impl
SizeDepType< NQ >::template SegmentReturn< D >::ConstType JointMappedConfigSelector_impl(const Eigen::MatrixBase< D > &a) const
Definition: joint-mimic.hpp:787
pinocchio::JointDataMimicTpl::U_accessor
U_t U_accessor() const
Definition: joint-mimic.hpp:454
pinocchio::JointModelMimicTpl::jointRows_impl
SizeDepType< NV >::template RowsReturn< D >::Type jointRows_impl(Eigen::MatrixBase< D > &A) const
Definition: joint-mimic.hpp:845
pinocchio::JointModelBase::idx_v
int idx_v() const
Definition: joint-model-base.hpp:175
pinocchio::ForceSetTpl
Definition: force-set.hpp:14
pinocchio::v
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > const Eigen::MatrixBase< TangentVectorType > & v
Definition: joint-configuration.hpp:1084
pinocchio::JointDataMimicTpl::JointDataMimicTpl
JointDataMimicTpl()
Definition: joint-mimic.hpp:380
pinocchio::setIndexes
void setIndexes(JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel, JointIndex id, int q, int v, int vExtended)
Visit a JointModelTpl through JointSetIndexesVisitor to set the indexes of the joint in the kinematic...
pinocchio::traits< JointMimicTpl< _Scalar, _Options, JointCollectionTpl > >::D_t
Eigen::Matrix< Scalar, Eigen::Dynamic, Eigen::Dynamic, Options > D_t
Definition: joint-mimic.hpp:324
pinocchio::ConstraintForceSetOp
Return type of the Constraint::Transpose * ForceSet operation.
Definition: joint-motion-subspace-base.hpp:53
pinocchio::traits< JointMimicTpl< _Scalar, _Options, JointCollectionTpl > >::Constraint_t
ScaledJointMotionSubspaceTpl< Scalar, Options, MaxNVMimicked > Constraint_t
Definition: joint-mimic.hpp:317
pinocchio::JointModelMimicTpl::JointMappedVelocitySelector_impl
SizeDepType< NQ >::template SegmentReturn< D >::ConstType JointMappedVelocitySelector_impl(const Eigen::MatrixBase< D > &a) const
Definition: joint-mimic.hpp:803
pinocchio::JointModelMimicTpl::jointBlock_impl
SizeDepType< NV >::template BlockReturn< D >::ConstType jointBlock_impl(const Eigen::MatrixBase< D > &Mat) const
Definition: joint-mimic.hpp:855
pinocchio::ScaledJointMotionSubspaceTpl::Base
JointMotionSubspaceBase< ScaledJointMotionSubspaceTpl > Base
Definition: joint-mimic.hpp:98
pinocchio::ScaledJointMotionSubspaceTpl::matrix_impl
DenseBase & matrix_impl()
Definition: joint-mimic.hpp:198
pinocchio::JointModelMimicTpl::JointCollection
JointCollectionTpl< Scalar, Options > JointCollection
Definition: joint-mimic.hpp:564
pinocchio::JointDataMimicTpl::v_accessor
Motion_t v_accessor() const
Definition: joint-mimic.hpp:444
pinocchio::traits< JointDataMimicTpl< _Scalar, Options, JointCollectionTpl > >::JointDerived
JointMimicTpl< _Scalar, Options, JointCollectionTpl > JointDerived
Definition: joint-mimic.hpp:356
pinocchio::JointModelBase::idx_q
int idx_q() const
Definition: joint-model-base.hpp:171
joint-basic-visitors.hpp
pinocchio::ScaledJointMotionSubspaceTpl::TransposeConst::ref
const ScaledJointMotionSubspaceTpl & ref
Definition: joint-mimic.hpp:164
pinocchio::JointDataTpl::Dinv
D_t Dinv() const
Definition: joint-generic.hpp:148
pinocchio::ConstraintForceSetOp< ScaledJointMotionSubspaceTpl< _Scalar, _Options, _MaxDim >, ForceSet >::ReturnType
ScalarMatrixProduct< _Scalar, RefReturnType >::type ReturnType
Definition: joint-mimic.hpp:83
pinocchio::ScalarMatrixProduct
Definition: math/matrix.hpp:78
pinocchio::JointModelMimicTpl::jointBlock_impl
SizeDepType< NV >::template BlockReturn< D >::Type jointBlock_impl(Eigen::MatrixBase< D > &Mat) const
Definition: joint-mimic.hpp:864
pinocchio::ScaledJointMotionSubspaceTpl::MaxDim
@ MaxDim
Definition: joint-mimic.hpp:96
pinocchio::ScaledJointMotionSubspaceTpl::SE3ActionReturnType
SE3GroupAction< RefJointMotionSubspace >::ReturnType SE3ActionReturnType
Definition: joint-mimic.hpp:103
pinocchio::ForceDense
Definition: context/casadi.hpp:35
pinocchio::impl::LhsMultiplicationOp< Eigen::MatrixBase< M6Like >, ScaledJointMotionSubspaceTpl< S2, O2, MD2 > >::ReturnType
MultiplicationOp< Eigen::MatrixBase< M6Like >, Constraint >::ReturnType ReturnType
Definition: joint-mimic.hpp:282
pinocchio::traits< JointMimicTpl< _Scalar, _Options, JointCollectionTpl > >::DTypeRef
D_t DTypeRef
Definition: joint-mimic.hpp:338
pinocchio::traits< ScaledJointMotionSubspaceTpl< _Scalar, _Options, _MaxDim > >::Scalar
traits< RefJointMotionSubspace >::Scalar Scalar
Definition: joint-mimic.hpp:28
pinocchio::traits< JointMimicTpl< _Scalar, _Options, JointCollectionTpl > >::BiasTypeRef
Bias_t BiasTypeRef
Definition: joint-mimic.hpp:334
pinocchio::ScaledJointMotionSubspaceTpl::transpose
TransposeConst transpose() const
Definition: joint-mimic.hpp:187
pinocchio::JointDataMimicTpl::StU_accessor
D_t StU_accessor() const
Definition: joint-mimic.hpp:469
pinocchio::JointModelMimicTpl::scaling
const Scalar & scaling() const
Definition: joint-mimic.hpp:758
pinocchio::ScaledJointMotionSubspaceTpl::se3ActionInverse
SE3ActionReturnType se3ActionInverse(const SE3Tpl< S1, O1 > &m) const
Definition: joint-mimic.hpp:152
pinocchio::JointModelMimicTpl::hasConfigurationLimit
const std::vector< bool > hasConfigurationLimit() const
Definition: joint-mimic.hpp:676
pinocchio::ScaledJointMotionSubspaceTpl::isEqual
bool isEqual(const ScaledJointMotionSubspaceTpl &other) const
Definition: joint-mimic.hpp:229
PINOCCHIO_CONSTRAINT_TYPEDEF_TPL
#define PINOCCHIO_CONSTRAINT_TYPEDEF_TPL(DERIVED)
Definition: joint-motion-subspace-base.hpp:36
pinocchio::ConstraintForceOp< ScaledJointMotionSubspaceTpl< _Scalar, _Options, _MaxDim >, ForceDerived >::RefJointMotionSubspace
ScaledJointMotionSubspaceTpl< _Scalar, _Options, _MaxDim >::RefJointMotionSubspace RefJointMotionSubspace
Definition: joint-mimic.hpp:69
fwd.hpp
pinocchio::traits< JointMimicTpl< _Scalar, _Options, JointCollectionTpl > >::ConfigVectorTypeRef
ConfigVector_t & ConfigVectorTypeRef
Definition: joint-mimic.hpp:346
pinocchio::impl::LhsMultiplicationOp< InertiaTpl< S1, O1 >, ScaledJointMotionSubspaceTpl< S2, O2, MD2 > >::ReturnType
MultiplicationOp< Inertia, Constraint >::ReturnType ReturnType
Definition: joint-mimic.hpp:258
pinocchio::JointIndex
Index JointIndex
Definition: multibody/fwd.hpp:26
pinocchio::JointModelMimicTpl
Definition: multibody/joint/fwd.hpp:155
Y
Y
pinocchio::JointDataMimicTpl::v_transformed
const TangentVector_t & v_transformed() const
Definition: joint-mimic.hpp:515
codegen-rnea.nv
nv
Definition: codegen-rnea.py:19
pinocchio::traits< JointMimicTpl< _Scalar, _Options, JointCollectionTpl > >::TansformTypeConstRef
Transformation_t TansformTypeConstRef
Definition: joint-mimic.hpp:329
pinocchio::ScaledJointMotionSubspaceTpl::scaling
Scalar & scaling()
Definition: joint-mimic.hpp:215
pinocchio::JointModelMimicTpl::setMimicIndexes
void setMimicIndexes(JointIndex id, int q, int v, int vExtended)
Specific way for mimic joints to set the mimicked q,v indexes. Used for manipulating tree (e....
Definition: joint-mimic.hpp:659
pinocchio::ScaledJointMotionSubspaceTpl::ScaledJointMotionSubspaceTpl
ScaledJointMotionSubspaceTpl(const ConstraintTpl &constraint, const Scalar &scaling_factor)
Definition: joint-mimic.hpp:117
pinocchio::traits
Common traits structure to fully define base classes for CRTP.
Definition: fwd.hpp:71
pinocchio::JointModelMimicTpl::m_scaling
Scalar m_scaling
Definition: joint-mimic.hpp:779
pinocchio::JointDataMimicTpl::JointDataMimicTpl
JointDataMimicTpl(const RefJointData &jdata, const Scalar &scaling, const int &nq, const int &nv)
Definition: joint-mimic.hpp:389
pinocchio::JointModelMimicTpl::offset
Scalar & offset()
Definition: joint-mimic.hpp:771
pinocchio::JointDataMimicTpl::joint_q_transformed
ConfigVector_t joint_q_transformed
Transformed configuration vector.
Definition: joint-mimic.hpp:531
pinocchio::JointModelMimicTpl::scaling
Scalar & scaling()
Definition: joint-mimic.hpp:762
pinocchio::JointModelMimicTpl::calc_aba
void calc_aba(JointDataDerived &, const Eigen::MatrixBase< VectorLike > &, const Eigen::MatrixBase< Matrix6Like > &, const bool) const
Definition: joint-mimic.hpp:713
pinocchio::JointDataMimicTpl::Dinv_accessor
D_t Dinv_accessor() const
Definition: joint-mimic.hpp:459
pinocchio::JointModelBase::nv
int nv() const
Definition: joint-model-base.hpp:144
pinocchio::JointModelMimicTpl::calc
EIGEN_DONT_INLINE void calc(JointDataDerived &jdata, const typename Eigen::MatrixBase< ConfigVector > &qs) const
Definition: joint-mimic.hpp:688
Base
pinocchio::SizeDepType::block
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)
Definition: matrix-block.hpp:104
pinocchio::ConstraintForceOp< ScaledJointMotionSubspaceTpl< _Scalar, _Options, _MaxDim >, ForceDerived >::RefReturnType
ConstraintForceOp< RefJointMotionSubspace, ForceDerived >::ReturnType RefReturnType
Definition: joint-mimic.hpp:71
pinocchio::ScaledJointMotionSubspaceTpl::__mult__
JointMotion __mult__(const Eigen::MatrixBase< VectorLike > &v) const
Definition: joint-mimic.hpp:137
pinocchio::JointDataMimicTpl
Definition: multibody/joint/fwd.hpp:162
pinocchio::MotionTpl< Scalar, Options >
pinocchio::CastType
Type of the cast of a class C templated by Scalar and Options, to a new NewScalar type....
Definition: fwd.hpp:99
pinocchio::JointDataTpl::shortname
std::string shortname() const
Definition: joint-generic.hpp:224
pinocchio::traits< JointDataMimicTpl< _Scalar, Options, JointCollectionTpl > >::Scalar
_Scalar Scalar
Definition: joint-mimic.hpp:357
pinocchio::JointDataMimicTpl::jdata
const RefJointData & jdata() const
Definition: joint-mimic.hpp:476
pinocchio::JointDataMimicTpl::JointDerived
JointMimicTpl< _Scalar, _Options, JointCollectionTpl > JointDerived
Definition: joint-mimic.hpp:374
dcrba.NV
NV
Definition: dcrba.py:536
pinocchio::traits< JointMimicTpl< _Scalar, _Options, JointCollectionTpl > >::U_t
Eigen::Matrix< Scalar, 6, Eigen::Dynamic, Options > U_t
Definition: joint-mimic.hpp:323
pinocchio::traits< ScaledJointMotionSubspaceTpl< _Scalar, _Options, _MaxDim > >::JointMotion
traits< RefJointMotionSubspace >::JointMotion JointMotion
Definition: joint-mimic.hpp:38
pinocchio::traits< JointMimicTpl< _Scalar, _Options, JointCollectionTpl > >::UTypeConstRef
U_t UTypeConstRef
Definition: joint-mimic.hpp:335
pinocchio::traits< JointMimicTpl< _Scalar, _Options, JointCollectionTpl > >::JointModelDerived
JointModelMimicTpl< Scalar, Options, JointCollectionTpl > JointModelDerived
Definition: joint-mimic.hpp:315
dpendulum.NQ
int NQ
Definition: dpendulum.py:9
pinocchio::ScaledJointMotionSubspaceTpl::nv_impl
int nv_impl() const
Definition: joint-mimic.hpp:157
isEqual
virtual bool isEqual(const CollisionGeometry &other) const=0
pinocchio::JointDataTpl::M
Transformation_t M() const
Definition: joint-generic.hpp:130
meshcat-viewer.qs
qs
Definition: meshcat-viewer.py:128
pinocchio::JointModelMimicTpl::createData
JointDataDerived createData() const
Definition: joint-mimic.hpp:670
pinocchio::JointDataMimicTpl::joint_v_accessor
TangentVectorTypeRef joint_v_accessor()
Definition: joint-mimic.hpp:502
pinocchio::impl::LhsMultiplicationOp< InertiaTpl< S1, O1 >, ScaledJointMotionSubspaceTpl< S2, O2, MD2 > >::run
static ReturnType run(const Inertia &Y, const Constraint &scaled_constraint)
Definition: joint-mimic.hpp:260
pinocchio::JointModelMimicTpl::nv_impl
int nv_impl() const
Definition: joint-mimic.hpp:625
pinocchio::ConstraintForceSetOp< ScaledJointMotionSubspaceTpl< _Scalar, _Options, _MaxDim >, ForceSet >::RefJointMotionSubspace
ScaledJointMotionSubspaceTpl< _Scalar, _Options, _MaxDim >::RefJointMotionSubspace RefJointMotionSubspace
Definition: joint-mimic.hpp:80
pinocchio::JointMotionSubspaceTpl
Definition: joint-motion-subspace-generic.hpp:65
pinocchio::ScalarCast
Cast scalar type from type FROM to type TO.
Definition: fwd.hpp:105
pinocchio::JointModelTpl::shortname
std::string shortname() const
Definition: joint-generic.hpp:410
pinocchio
Main pinocchio namespace.
Definition: timings.cpp:27
pinocchio::JointModelTpl::calc
void calc(JointDataDerived &data, const Eigen::MatrixBase< ConfigVector > &q) const
Definition: joint-generic.hpp:353
pinocchio::JointModelBase::id
JointIndex id() const
Definition: joint-model-base.hpp:183


pinocchio
Author(s):
autogenerated on Fri Mar 7 2025 03:41:56