pinocchio  2.7.1
A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
joint-generic.hpp
1 //
2 // Copyright (c) 2016-2021 CNRS INRIA
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;
33  typedef JointDataTpl<Scalar,Options,JointCollectionTpl> JointDataDerived;
34  typedef JointModelTpl<Scalar,Options,JointCollectionTpl> JointModelDerived;
35 
36  typedef ConstraintTpl<Eigen::Dynamic,Scalar,Options> Constraint_t;
37  typedef SE3Tpl<Scalar,Options> Transformation_t;
38  typedef MotionTpl<Scalar,Options> Motion_t;
39  typedef MotionTpl<Scalar,Options> Bias_t;
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 Constraint_t ConstraintTypeRef;
48  typedef Transformation_t TansformTypeConstRef;
49  typedef Transformation_t TansformTypeRef;
50  typedef Motion_t MotionTypeConstRef;
51  typedef Motion_t MotionTypeRef;
52  typedef Bias_t BiasTypeConstRef;
53  typedef Bias_t BiasTypeRef;
54  typedef U_t UTypeConstRef;
55  typedef U_t UTypeRef;
56  typedef D_t DTypeConstRef;
57  typedef D_t DTypeRef;
58  typedef UD_t UDTypeConstRef;
59  typedef UD_t UDTypeRef;
60 
61  typedef Eigen::Matrix<Scalar,Eigen::Dynamic,1,Options> ConfigVector_t;
62  typedef Eigen::Matrix<Scalar,Eigen::Dynamic,1,Options> TangentVector_t;
63  };
64 
65  template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
66  struct traits< JointDataTpl<Scalar,Options,JointCollectionTpl> >
67  { typedef JointTpl<Scalar,Options,JointCollectionTpl> JointDerived; };
68 
69  template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
70  struct traits< JointModelTpl<Scalar,Options,JointCollectionTpl> >
71  { typedef JointTpl<Scalar,Options,JointCollectionTpl> JointDerived; };
72 
73  template<typename _Scalar, int _Options, template<typename S, int O> class JointCollectionTpl>
74  struct JointDataTpl
75  : public JointDataBase< JointDataTpl<_Scalar,_Options,JointCollectionTpl> >
76  , JointCollectionTpl<_Scalar,_Options>::JointDataVariant
77  {
78  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
79 
80  typedef JointTpl<_Scalar,_Options,JointCollectionTpl> JointDerived;
81  typedef JointDataBase<JointDataTpl> Base;
82 
83  PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(JointDerived);
84 
85  typedef JointCollectionTpl<_Scalar,_Options> JointCollection;
86  typedef typename JointCollection::JointDataVariant JointDataVariant;
87 
88  using Base::operator==;
89  using Base::operator!=;
90 
91  JointDataVariant & toVariant() { return *static_cast<JointDataVariant*>(this); }
92  const JointDataVariant & toVariant() const { return *static_cast<const JointDataVariant*>(this); }
93 
94  Constraint_t S() const { return constraint_xd(*this); }
95  Transformation_t M() const { return joint_transform(*this); }
96  Motion_t v() const { return motion(*this); }
97  Bias_t c() const { return bias(*this); }
98 
99  // [ABA CCRBA]
100  U_t U() const { return u_inertia(*this); }
101  D_t Dinv() const { return dinv_inertia(*this); }
102  UD_t UDinv() const { return udinv_inertia(*this); }
103 
104  JointDataTpl()
105  : JointDataVariant()
106  {}
107 
108  JointDataTpl(const JointDataVariant & jdata_variant)
109  : JointDataVariant(jdata_variant)
110  {}
111 
112  template<typename JointDataDerived>
113  JointDataTpl(const JointDataBase<JointDataDerived> & jdata)
114  : JointCollection::JointDataVariant((JointDataVariant)jdata.derived())
115  {
116  BOOST_MPL_ASSERT((boost::mpl::contains<typename JointDataVariant::types,JointDataDerived>));
117  }
118 
119  // Define all the standard accessors
120  Constraint_t S_accessor() const { return S(); }
121  Transformation_t M_accessor() const { return M(); }
122  Motion_t v_accessor() const { return v(); }
123  Bias_t c_accessor() const { return c(); }
124  U_t U_accessor() const { return U(); }
125  D_t Dinv_accessor() const { return Dinv(); }
126  UD_t UDinv_accessor() const { return UDinv(); }
127 
128  static std::string classname() { return "JointData"; }
129  std::string shortname() const { return ::pinocchio::shortname(*this); }
130 
131  template<typename JointDataDerived>
132  bool isEqual(const JointDataBase<JointDataDerived> & other) const
133  {
134  return ::pinocchio::isEqual(*this,other.derived());
135  }
136 
137  bool isEqual(const JointDataTpl & other) const
138  {
139  return Base::isEqual(other) && toVariant() == other.toVariant();
140  }
141 
142  bool operator==(const JointDataTpl & other) const
143  {
144  return isEqual(other);
145  }
146 
147  bool operator!=(const JointDataTpl & other) const
148  {
149  return !(*this == other);
150  }
151 
152  };
153 
154  template<typename NewScalar, typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
155  struct CastType< NewScalar, JointModelTpl<Scalar,Options,JointCollectionTpl> >
156  {
157  typedef JointModelTpl<NewScalar,Options,JointCollectionTpl> type;
158  };
159 
160  template<typename _Scalar, int _Options, template<typename S, int O> class JointCollectionTpl>
161  struct JointModelTpl
162  : JointModelBase< JointModelTpl<_Scalar,_Options,JointCollectionTpl> >
163  , JointCollectionTpl<_Scalar,_Options>::JointModelVariant
164  {
165  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
166 
167  typedef JointTpl<_Scalar,_Options,JointCollectionTpl> JointDerived;
168 
169  PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived);
170  PINOCCHIO_JOINT_USE_INDEXES(JointModelTpl);
171 
172  typedef JointCollectionTpl<Scalar,Options> JointCollection;
173  typedef typename JointCollection::JointDataVariant JointDataVariant;
174  typedef typename JointCollection::JointModelVariant JointModelVariant;
175 
176  using Base::id;
177  using Base::setIndexes;
178  using Base::operator==;
179  using Base::operator!=;
180 
181  JointModelTpl() : JointModelVariant() {}
182 
183  JointModelTpl(const JointModelVariant & jmodel_variant)
184  : JointCollection::JointModelVariant(jmodel_variant)
185  {
186  }
187 
188  const std::vector<bool> hasConfigurationLimit() const
189  {
190  return ::pinocchio::hasConfigurationLimit(*this);
191  }
192 
193  const std::vector<bool> hasConfigurationLimitInTangent() const
194  {
195  return ::pinocchio::hasConfigurationLimitInTangent(*this);
196  }
197 
198  template<typename JointModelDerived>
199  JointModelTpl(const JointModelBase<JointModelDerived> & jmodel)
200  : JointModelVariant((JointModelVariant)jmodel.derived())
201  {
202  BOOST_MPL_ASSERT((boost::mpl::contains<typename JointModelVariant::types,JointModelDerived>));
203  }
204 
205  JointModelVariant & toVariant()
206  { return *static_cast<JointModelVariant*>(this); }
207 
208  const JointModelVariant & toVariant() const
209  { return *static_cast<const JointModelVariant*>(this); }
210 
211  JointDataDerived createData() const
212  { return ::pinocchio::createData<Scalar,Options,JointCollectionTpl>(*this); }
213 
214  template<typename JointModelDerived>
215  bool isEqual(const JointModelBase<JointModelDerived> & other) const
216  {
217  return ::pinocchio::isEqual(*this,other.derived());;
218  }
219 
220  template<typename JointModelDerived>
221  bool hasSameIndexes(const JointModelBase<JointModelDerived> & other) const
222  {
223  return ::pinocchio::hasSameIndexes(*this,other.derived());
224  }
225 
226  bool isEqual(const JointModelTpl & other) const
227  {
228  return Base::isEqual(other) && toVariant() == other.toVariant();
229  }
230 
231  bool operator==(const JointModelTpl & other) const
232  {
233  return isEqual(other);
234  }
235 
236  bool operator!=(const JointModelTpl & other) const
237  {
238  return !(*this == other);
239  }
240 
241  template<typename ConfigVector>
242  void calc(JointDataDerived & data,
243  const Eigen::MatrixBase<ConfigVector> & q) const
244  { calc_zero_order(*this,data,q); }
245 
246  template<typename ConfigVector, typename TangentVector>
247  void calc(JointDataDerived & data,
248  const Eigen::MatrixBase<ConfigVector> & q,
249  const Eigen::MatrixBase<TangentVector> & v) const
250  { calc_first_order(*this,data,q,v); }
251 
252  template<typename Matrix6Like>
253  void calc_aba(JointDataDerived & data, const Eigen::MatrixBase<Matrix6Like> & I, const bool update_I) const
254  { ::pinocchio::calc_aba(*this,data,PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like,I),update_I); }
255 
256  std::string shortname() const { return ::pinocchio::shortname(*this); }
257  static std::string classname() { return "JointModel"; }
258 
259  int nq_impl() const { return ::pinocchio::nq(*this); }
260  int nv_impl() const { return ::pinocchio::nv(*this); }
261 
262  int idx_q_impl() const { return ::pinocchio::idx_q(*this); }
263  int idx_v_impl() const { return ::pinocchio::idx_v(*this); }
264 
265  JointIndex id_impl() const { return ::pinocchio::id(*this); }
266 
267  void setIndexes(JointIndex id, int nq, int nv)
268  {
269  ::pinocchio::setIndexes(*this, id, nq, nv);
270  }
271 
273  template<typename NewScalar>
274  JointModelTpl<NewScalar,Options,JointCollectionTpl> cast() const
275  {
276  return cast_joint<NewScalar,Scalar,Options,JointCollectionTpl>(*this);
277  }
278  };
279 
280  typedef PINOCCHIO_ALIGNED_STD_VECTOR(JointData) JointDataVector;
281  typedef PINOCCHIO_ALIGNED_STD_VECTOR(JointModel) JointModelVector;
282 
283 
284  template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl, typename JointDataDerived>
285  bool operator==(const JointDataBase<JointDataDerived> & joint_data,
286  const JointDataTpl<Scalar,Options,JointCollectionTpl> & joint_data_generic)
287  {
288  return joint_data_generic == joint_data.derived();
289  }
290 
291 
292  template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl, typename JointDataDerived>
293  bool operator!=(const JointDataBase<JointDataDerived> & joint_data,
294  const JointDataTpl<Scalar,Options,JointCollectionTpl> & joint_data_generic)
295  {
296  return joint_data_generic != joint_data.derived();
297  }
298 
299  template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl, typename JointModelDerived>
300  bool operator==(const JointModelBase<JointModelDerived> & joint_model,
301  const JointModelTpl<Scalar,Options,JointCollectionTpl> & joint_model_generic)
302  {
303  return joint_model_generic == joint_model.derived();
304  }
305 
306  template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl, typename JointModelDerived>
307  bool operator!=(const JointModelBase<JointModelDerived> & joint_model,
308  const JointModelTpl<Scalar,Options,JointCollectionTpl> & joint_model_generic)
309  {
310  return joint_model_generic != joint_model.derived();
311  }
312 
313 } // namespace pinocchio
314 
315 #endif // ifndef __pinocchio_joint_generic_hpp__
pinocchio::JointModelBase
Definition: joint-model-base.hpp:67
pinocchio::JointTpl
Definition: joint-generic.hpp:19
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::SE3Tpl< Scalar, Options >
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::JointDataBase
Definition: joint-data-base.hpp:82
pinocchio::JointDataTpl
Definition: fwd.hpp:97
pinocchio::udinv_inertia
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...
pinocchio::JointDataBase::isEqual
bool isEqual(const JointDataBase &other) const
&#160;
Definition: joint-data-base.hpp:130
pinocchio::JointModelTpl
Definition: fwd.hpp:93
pinocchio::calc_first_order
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...
pinocchio::constraint_xd
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...
pinocchio::u_inertia
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...
pinocchio::motion
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.
pinocchio::traits
Common traits structure to fully define base classes for CRTP.
Definition: fwd.hpp:44
pinocchio::calc_zero_order
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...
pinocchio::JointModelTpl::cast
JointModelTpl< NewScalar, Options, JointCollectionTpl > cast() const
Definition: joint-generic.hpp:274
pinocchio::joint_transform
SE3Tpl< Scalar, Options > joint_transform(const JointDataTpl< Scalar, Options, JointCollectionTpl > &jdata)
Visit a JointDataTpl through JointTransformVisitor to get the joint internal transform (transform bet...
pinocchio::bias
MotionTpl< Scalar, Options > bias(const JointDataTpl< Scalar, Options, JointCollectionTpl > &jdata)
Visit a JointDataTpl through JointBiasVisitor to get the joint bias as a dense motion.
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:55
pinocchio::dinv_inertia
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...
pinocchio
Main pinocchio namespace.
Definition: treeview.dox:11
pinocchio::ConstraintTpl
Definition: constraint-generic.hpp:42