pinocchio  2.2.1-dirty
joint-generic.hpp
1 //
2 // Copyright (c) 2016,2018 CNRS
3 //
4 
5 #ifndef __pinocchio_joint_generic_hpp__
6 #define __pinocchio_joint_generic_hpp__
7 
8 #include "pinocchio/multibody/joint/joint-collection.hpp"
9 #include "pinocchio/multibody/joint/joint-composite.hpp"
10 #include "pinocchio/multibody/joint/joint-basic-visitors.hxx"
11 #include "pinocchio/container/aligned-vector.hpp"
12 
13 #include <boost/mpl/contains.hpp>
14 
15 namespace pinocchio
16 {
17 
18  template<typename Scalar, int Options = 0, template<typename S, int O> class JointCollectionTpl = JointCollectionDefaultTpl>
19  struct JointTpl;
20  typedef JointTpl<double> Joint;
21 
22  template<typename _Scalar, int _Options, template<typename S, int O> class JointCollectionTpl>
23  struct traits< JointTpl<_Scalar,_Options,JointCollectionTpl> >
24  {
25  enum {
26  Options = _Options,
27  NQ = Eigen::Dynamic, // Dynamic because unknown at compile time
28  NV = Eigen::Dynamic
29  };
30 
31  typedef _Scalar Scalar;
32  typedef JointCollectionTpl<Scalar,Options> JointCollection;
35 
40 
41  // [ABA]
42  typedef Eigen::Matrix<Scalar,6,Eigen::Dynamic,Options> U_t;
43  typedef Eigen::Matrix<Scalar,Eigen::Dynamic,Eigen::Dynamic,Options> D_t;
44  typedef Eigen::Matrix<Scalar,6,Eigen::Dynamic,Options> UD_t;
45 
46  typedef Constraint_t ConstraintTypeConstRef;
47  typedef Transformation_t TansformTypeConstRef;
48  typedef Motion_t MotionTypeConstRef;
49  typedef Bias_t BiasTypeConstRef;
50  typedef U_t UTypeConstRef;
51  typedef U_t UTypeRef;
52  typedef D_t DTypeConstRef;
53  typedef UD_t UDTypeConstRef;
54 
55  typedef Eigen::Matrix<Scalar,Eigen::Dynamic,1,Options> ConfigVector_t;
56  typedef Eigen::Matrix<Scalar,Eigen::Dynamic,1,Options> TangentVector_t;
57  };
58 
59  template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
60  struct traits< JointDataTpl<Scalar,Options,JointCollectionTpl> >
62 
63  template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
64  struct traits< JointModelTpl<Scalar,Options,JointCollectionTpl> >
66 
67  template<typename _Scalar, int _Options, template<typename S, int O> class JointCollectionTpl>
68  struct JointDataTpl
69  : public JointDataBase< JointDataTpl<_Scalar,_Options,JointCollectionTpl> >
70  , JointCollectionTpl<_Scalar,_Options>::JointDataVariant
71  {
72  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
73 
75  typedef JointDataBase<JointDataTpl> Base;
76 
77  PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(JointDerived);
78 
79  typedef JointCollectionTpl<_Scalar,_Options> JointCollection;
80  typedef typename JointCollection::JointDataVariant JointDataVariant;
81 
82  JointDataVariant & toVariant() { return *static_cast<JointDataVariant*>(this); }
83  const JointDataVariant & toVariant() const { return *static_cast<const JointDataVariant*>(this); }
84 
85  Constraint_t S() const { return constraint_xd(*this); }
86  Transformation_t M() const { return joint_transform(*this); }
87  Motion_t v() const { return motion(*this); }
88  Bias_t c() const { return bias(*this); }
89 
90  // [ABA CCRBA]
91  U_t U() const { return u_inertia(*this); }
92  D_t Dinv() const { return dinv_inertia(*this); }
93  UD_t UDinv() const { return udinv_inertia(*this); }
94 
95  JointDataTpl() : JointDataVariant() {}
96 
97  JointDataTpl(const JointDataVariant & jdata_variant)
98  : JointDataVariant(jdata_variant)
99  {}
100 
101  template<typename JointDataDerived>
103  : JointCollection::JointDataVariant((JointDataVariant)jdata.derived())
104  {
105  BOOST_MPL_ASSERT((boost::mpl::contains<typename JointDataVariant::types,JointDataDerived>));
106  }
107 
109  Constraint_t S_accessor() const { return S(); }
110  Transformation_t M_accessor() const { return M(); }
111  Motion_t v_accessor() const { return v(); }
112  Bias_t c_accessor() const { return c(); }
113  U_t U_accessor() const { return U(); }
114  D_t Dinv_accessor() const { return Dinv(); }
115  UD_t UDinv_accessor() const { return UDinv(); }
116 
117  static std::string classname() { return "JointData"; }
118  std::string shortname() const { return ::pinocchio::shortname(*this); }
119 
120  };
121 
122  template<typename NewScalar, typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
123  struct CastType< NewScalar, JointModelTpl<Scalar,Options,JointCollectionTpl> >
124  {
126  };
127 
128  template<typename _Scalar, int _Options, template<typename S, int O> class JointCollectionTpl>
129  struct JointModelTpl
130  : JointModelBase< JointModelTpl<_Scalar,_Options,JointCollectionTpl> >
131  , JointCollectionTpl<_Scalar,_Options>::JointModelVariant
132  {
133  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
134 
136 
137  PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived);
138  PINOCCHIO_JOINT_USE_INDEXES;
139 
140  typedef JointCollectionTpl<Scalar,Options> JointCollection;
141  typedef typename JointCollection::JointDataVariant JointDataVariant;
142  typedef typename JointCollection::JointModelVariant JointModelVariant;
143 
144  using Base::id;
145  using Base::setIndexes;
146  using Base::operator==;
147  using Base::operator!=;
148 
149  JointModelTpl() : JointModelVariant() {}
150 
151  JointModelTpl(const JointModelVariant & jmodel_variant)
152  : JointCollection::JointModelVariant(jmodel_variant)
153  {}
154 
155  template<typename JointModelDerived>
157  : JointModelVariant((JointModelVariant)jmodel.derived())
158  {
159  BOOST_MPL_ASSERT((boost::mpl::contains<typename JointModelVariant::types,JointModelDerived>));
160  }
161 
162  JointModelVariant & toVariant()
163  { return *static_cast<JointModelVariant*>(this); }
164 
165  const JointModelVariant & toVariant() const
166  { return *static_cast<const JointModelVariant*>(this); }
167 
168  JointDataDerived createData() const
169  { return ::pinocchio::createData<Scalar,Options,JointCollectionTpl>(*this); }
170 
171  using Base::isEqual;
172  bool isEqual(const JointModelTpl & other) const
173  {
174  return Base::isEqual(other)
175  && toVariant() == other.toVariant();
176  }
177 
178  template<typename ConfigVector>
179  void calc(JointDataDerived & data,
180  const Eigen::MatrixBase<ConfigVector> & q) const
181  { calc_zero_order(*this,data,q); }
182 
183  template<typename ConfigVector, typename TangentVector>
184  void calc(JointDataDerived & data,
185  const Eigen::MatrixBase<ConfigVector> & q,
186  const Eigen::MatrixBase<TangentVector> & v) const
187  { calc_first_order(*this,data,q,v); }
188 
189  template<typename Matrix6Like>
190  void calc_aba(JointDataDerived & data, const Eigen::MatrixBase<Matrix6Like> & I, const bool update_I) const
191  { ::pinocchio::calc_aba(*this,data,PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like,I),update_I); }
192 
193  std::string shortname() const { return ::pinocchio::shortname(*this); }
194  static std::string classname() { return "JointModel"; }
195 
196  int nq_impl() const { return ::pinocchio::nq(*this); }
197  int nv_impl() const { return ::pinocchio::nv(*this); }
198 
199  int idx_q_impl() const { return ::pinocchio::idx_q(*this); }
200  int idx_v_impl() const { return ::pinocchio::idx_v(*this); }
201 
202  JointIndex id_impl() const { return ::pinocchio::id(*this); }
203 
204  void setIndexes(JointIndex id, int nq, int nv)
205  { ::pinocchio::setIndexes(*this,id, nq, nv); }
206 
208  template<typename NewScalar>
210  {
211  return cast_joint<NewScalar,Scalar,Options,JointCollectionTpl>(*this);
212  }
213  };
214 
217 
218 } // namespace pinocchio
219 
220 #endif // ifndef __pinocchio_joint_generic_hpp__
int nv(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointNvVisitor to get the dimension of the joint tangent space...
MotionTpl< Scalar, Options > motion(const JointDataTpl< Scalar, Options, JointCollectionTpl > &jdata)
Visit a JointDataTpl through JointMotionVisitor to get the joint internal motion as a dense motion...
int nq(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointNqVisitor to get the dimension of the joint configuration space...
JointDataTpl< Scalar, Options, JointCollectionTpl > createData(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through CreateData visitor to create a JointDataTpl.
SE3Tpl< Scalar, Options > joint_transform(const JointDataTpl< Scalar, Options, JointCollectionTpl > &jdata)
Visit a JointDataTpl through JointTransformVisitor to get the joint internal transform (transform bet...
Eigen::Matrix< Scalar, 6, Eigen::Dynamic, Options > udinv_inertia(const JointDataTpl< Scalar, Options, JointCollectionTpl > &jdata)
Visit a JointDataTpl through JointUDInvInertiaVisitor to get U*D^{-1} matrix of the inertia matrix de...
std::string shortname(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointShortnameVisitor to get the shortname of the derived joint model...
Eigen::Matrix< Scalar, 6, Eigen::Dynamic, Options > u_inertia(const JointDataTpl< Scalar, Options, JointCollectionTpl > &jdata)
Visit a JointDataTpl through JointUInertiaVisitor to get the U matrix of the inertia matrix decomposi...
Main pinocchio namespace.
Definition: treeview.dox:24
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...
Constraint_t S_accessor() const
Define all the standard accessors.
MotionTpl< Scalar, Options > bias(const JointDataTpl< Scalar, Options, JointCollectionTpl > &jdata)
Visit a JointDataTpl through JointBiasVisitor to get the joint bias as a dense motion.
void calc_first_order(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel, JointDataTpl< Scalar, Options, JointCollectionTpl > &jdata, const Eigen::MatrixBase< ConfigVectorType > &q, const Eigen::MatrixBase< TangentVectorType > &v)
Visit a JointModelTpl and the corresponding JointDataTpl through JointCalcFirstOrderVisitor to comput...
Common traits structure to fully define base classes for CRTP.
Definition: spatial/fwd.hpp:32
Type of the cast of a class C templated by Scalar and Options, to a new NewScalar type...
Definition: spatial/fwd.hpp:43
Eigen::Matrix< Scalar, Eigen::Dynamic, Eigen::Dynamic, Options > dinv_inertia(const JointDataTpl< Scalar, Options, JointCollectionTpl > &jdata)
Visit a JointDataTpl through JointDInvInertiaVisitor to get the D^{-1} matrix of the inertia matrix d...
ConstraintTpl< Eigen::Dynamic, Scalar, Options > constraint_xd(const JointDataTpl< Scalar, Options, JointCollectionTpl > &jdata)
Visit a JointDataVariant through JointConstraintVisitor to get the joint constraint as a dense constr...
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...
JointModelTpl< NewScalar, Options, JointCollectionTpl > cast() const
void calc_zero_order(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel, JointDataTpl< Scalar, Options, JointCollectionTpl > &jdata, const Eigen::MatrixBase< ConfigVectorType > &q)
Visit a JointModelTpl and the corresponding JointDataTpl through JointCalcZeroOrderVisitor to compute...