joint-generic.hpp
Go to the documentation of this file.
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;
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 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> >
68 
69  template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
70  struct traits< JointModelTpl<Scalar,Options,JointCollectionTpl> >
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 
82 
84 
85  typedef JointCollectionTpl<_Scalar,_Options> JointCollection;
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 
105  : JointDataVariant()
106  {}
107 
108  JointDataTpl(const JointDataVariant & jdata_variant)
109  : JointDataVariant(jdata_variant)
110  {}
111 
112  template<typename JointDataDerived>
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  {
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 
168 
169  PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived);
171 
172  typedef JointCollectionTpl<Scalar,Options> JointCollection;
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  {
191  }
192 
193  const std::vector<bool> hasConfigurationLimitInTangent() const
194  {
196  }
197 
198  template<typename JointModelDerived>
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>
222  {
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 
266 
267  void setIndexes(JointIndex id, int nq, int nv)
268  {
269  ::pinocchio::setIndexes(*this, id, nq, nv);
270  }
271 
273  template<typename NewScalar>
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>
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>
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>
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>
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__
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > const Eigen::MatrixBase< TangentVectorType > & v
Constraint_t S_accessor() const
Motion_t v_accessor() const
bool isEqual(const JointModelTpl &other) const
int nv(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointNvVisitor to get the dimension of the joint tangent space...
static std::string classname()
int NQ
Definition: dpendulum.py:8
JointCollection::JointModelVariant JointModelVariant
JointCollection::JointDataVariant JointDataVariant
typedef PINOCCHIO_ALIGNED_STD_VECTOR(JointData) JointDataVector
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...
bool operator==(const JointModelTpl &other) const
int idx_q(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointIdxQVisitor to get the index in the full model configuration space...
void calc_aba(JointDataDerived &data, const Eigen::MatrixBase< Matrix6Like > &I, const bool update_I) const
Transformation_t M_accessor() const
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...
int nq(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointNqVisitor to get the dimension of the joint configuration space...
ConstraintTpl< Eigen::Dynamic, Scalar, Options > Constraint_t
JointIndex id_impl() const
SE3Tpl< Scalar, Options > joint_transform(const JointDataTpl< Scalar, Options, JointCollectionTpl > &jdata)
Visit a JointDataTpl through JointTransformVisitor to get the joint internal transform (transform bet...
JointIndex id(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointIdVisitor to get the index of the joint in the kinematic chain...
JointDataTpl(const JointDataVariant &jdata_variant)
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...
const std::vector< bool > hasConfigurationLimitInTangent(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointConfigurationLimitInTangentVisitor to get the configurations limit...
void calc(JointDataDerived &data, const Eigen::MatrixBase< ConfigVector > &q, const Eigen::MatrixBase< TangentVector > &v) const
JointCollection::JointDataVariant JointDataVariant
Vec3f c
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef JointTpl< _Scalar, _Options, JointCollectionTpl > JointDerived
JointDataTpl(const JointDataBase< JointDataDerived > &jdata)
bool hasSameIndexes(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel_generic, const JointModelBase< JointModelDerived > &jmodel)
Check whether JointModelTpl<Scalar,...> has the indexes than another JointModelDerived.
Transformation_t M() const
bool isEqual(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel_generic, const JointModelBase< JointModelDerived > &jmodel)
Visit a JointModelTpl<Scalar,...> to compare it to JointModelDerived.
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > & q
#define PINOCCHIO_EIGEN_CONST_CAST(TYPE, OBJ)
Macro for an automatic const_cast.
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef JointTpl< _Scalar, _Options, JointCollectionTpl > JointDerived
bool operator==(const JointDataBase< JointDataDerived > &joint_data, const JointDataTpl< Scalar, Options, JointCollectionTpl > &joint_data_generic)
JointCollectionTpl< _Scalar, _Options > JointCollection
SE3::Scalar Scalar
Definition: conversions.cpp:13
const JointDataVariant & toVariant() const
virtual bool isEqual(const CollisionGeometry &other) const=0
Constraint_t S() const
const JointModelVariant & toVariant() const
JointCollectionTpl< Scalar, Options > JointCollection
std::string shortname(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointShortnameVisitor to get the shortname of the derived joint model...
std::string shortname() const
#define PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(Joint)
Eigen::Matrix< Scalar, Eigen::Dynamic, Eigen::Dynamic, Options > D_t
bool operator!=(const JointModelTpl &other) const
bool hasSameIndexes(const JointModelBase< JointModelDerived > &other) const
#define PINOCCHIO_JOINT_USE_INDEXES(Joint)
JointModelTpl(const JointModelBase< JointModelDerived > &jmodel)
Eigen::Matrix< Scalar, 6, Eigen::Dynamic, Options > UD_t
bool operator!=(const JointDataBase< JointDataDerived > &joint_data, const JointDataTpl< Scalar, Options, JointCollectionTpl > &joint_data_generic)
U
Definition: ocp.py:61
JointDataTpl< Scalar, Options, JointCollectionTpl > JointDataDerived
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...
static std::string classname()
JointModelDerived & derived()
JointDataBase< JointDataTpl > Base
Eigen::Matrix< Scalar, 6, Eigen::Dynamic, Options > U_t
std::string shortname() const
void setIndexes(JointIndex id, int nq, int nv)
const std::vector< bool > hasConfigurationLimit() const
Main pinocchio namespace.
Definition: timings.cpp:28
const std::vector< bool > hasConfigurationLimit(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointConfigurationLimitVisitor to get the configurations limits...
JointCollectionDefault::JointDataVariant JointDataVariant
JointTpl< Scalar, Options, JointCollectionTpl > JointDerived
bool operator!=(const JointDataTpl &other) const
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...
JointDataVariant & toVariant()
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: src/fwd.hpp:44
JointTpl< double > Joint
const std::vector< bool > hasConfigurationLimitInTangent() const
Eigen::Matrix< Scalar, Eigen::Dynamic, 1, Options > TangentVector_t
JointModelVariant & toVariant()
JointModelTpl< NewScalar, Options, JointCollectionTpl > cast() const
bool isEqual(const JointModelBase< JointModelDerived > &other) const
JointModelTpl< Scalar, Options, JointCollectionTpl > JointModelDerived
JointCollectionDefault::JointModelVariant JointModelVariant
M
NV
Definition: dcrba.py:444
JointDataDerived createData() const
Type of the cast of a class C templated by Scalar and Options, to a new NewScalar type...
Definition: src/fwd.hpp:55
bool isEqual(const JointDataBase< JointDataDerived > &other) const
void calc(JointDataDerived &data, const Eigen::MatrixBase< ConfigVector > &q) const
JointTpl< Scalar, Options, JointCollectionTpl > JointDerived
JointModelTpl(const JointModelVariant &jmodel_variant)
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...
bool isEqual(const JointDataTpl &other) const
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...
Eigen::Matrix< Scalar, Eigen::Dynamic, 1, Options > ConfigVector_t
bool operator==(const JointDataTpl &other) 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...
#define PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(Joint)


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