5 #ifndef __pinocchio_multibody_joint_generic_hpp__
6 #define __pinocchio_multibody_joint_generic_hpp__
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"
13 #include <boost/mpl/contains.hpp>
20 int Options = context::Options,
21 template<
typename S,
int O>
class JointCollectionTpl = JointCollectionDefaultTpl>
25 template<
typename _Scalar,
int _Options,
template<
typename S,
int O>
class JointCollectionTpl>
33 NVExtended = Eigen::Dynamic
36 typedef _Scalar Scalar;
37 typedef JointCollectionTpl<Scalar, Options> JointCollection;
47 typedef Eigen::Matrix<Scalar, 6, Eigen::Dynamic, Options> U_t;
48 typedef Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, Options> D_t;
49 typedef Eigen::Matrix<Scalar, 6, Eigen::Dynamic, Options> UD_t;
59 typedef U_t UTypeConstRef;
61 typedef D_t DTypeConstRef;
63 typedef UD_t UDTypeConstRef;
64 typedef UD_t UDTypeRef;
66 typedef Eigen::Matrix<Scalar, Eigen::Dynamic, 1, Options> ConfigVector_t;
67 typedef Eigen::Matrix<Scalar, Eigen::Dynamic, 1, Options> TangentVector_t;
69 typedef ConfigVector_t ConfigVectorTypeConstRef;
70 typedef ConfigVector_t ConfigVectorTypeRef;
71 typedef TangentVector_t TangentVectorTypeConstRef;
72 typedef TangentVector_t TangentVectorTypeRef;
74 typedef boost::mpl::false_ is_mimicable_t;
77 template<
typename _Scalar,
int _Options,
template<
typename S,
int O>
class JointCollectionTpl>
84 template<
typename _Scalar,
int _Options,
template<
typename S,
int O>
class JointCollectionTpl>
91 template<
typename _Scalar,
int _Options,
template<
typename S,
int O>
class JointCollectionTpl>
93 :
public JointDataBase<JointDataTpl<_Scalar, _Options, JointCollectionTpl>>
94 , JointCollectionTpl<_Scalar, _Options>::JointDataVariant
96 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
103 typedef JointCollectionTpl<_Scalar, _Options> JointCollection;
104 typedef typename JointCollection::JointDataVariant JointDataVariant;
106 using Base::operator==;
107 using Base::operator!=;
109 JointDataVariant & toVariant()
111 return *
static_cast<JointDataVariant *
>(
this);
113 const JointDataVariant & toVariant()
const
115 return *
static_cast<const JointDataVariant *
>(
this);
118 ConfigVector_t joint_q()
const
122 TangentVector_t joint_v()
const
126 Constraint_t S()
const
130 Transformation_t M()
const
167 : JointDataVariant(jdata_variant)
171 template<
typename Jo
intDataDerived>
173 : JointCollection::JointDataVariant((JointDataVariant)jdata.derived())
175 BOOST_MPL_ASSERT((boost::mpl::contains<typename JointDataVariant::types, JointDataDerived>));
179 ConfigVector_t joint_q_accessor()
const
183 TangentVector_t joint_v_accessor()
const
187 Constraint_t S_accessor()
const
191 Transformation_t M_accessor()
const
195 Motion_t v_accessor()
const
199 Bias_t c_accessor()
const
203 U_t U_accessor()
const
207 D_t Dinv_accessor()
const
211 UD_t UDinv_accessor()
const
215 D_t StU_accessor()
const
220 static std::string classname()
224 std::string shortname()
const
226 return ::pinocchio::shortname(*
this);
229 template<
typename Jo
intDataDerived>
232 return ::pinocchio::isEqual(*
this, other.derived());
237 return Base::isEqual(other) && toVariant() == other.toVariant();
242 return isEqual(other);
247 return !(*
this == other);
255 template<
typename S,
int O>
class JointCollectionTpl>
261 template<
typename _Scalar,
int _Options,
template<
typename S,
int O>
class JointCollectionTpl>
263 :
JointModelBase<JointModelTpl<_Scalar, _Options, JointCollectionTpl>>
264 , JointCollectionTpl<_Scalar, _Options>::JointModelVariant
266 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
273 typedef JointCollectionTpl<Scalar, Options> JointCollection;
274 typedef typename JointCollection::JointDataVariant JointDataVariant;
275 typedef typename JointCollection::JointModelVariant JointModelVariant;
278 using Base::setIndexes;
279 using Base::operator==;
280 using Base::operator!=;
283 : JointModelVariant()
288 : JointCollection::JointModelVariant(jmodel_variant)
292 const std::vector<bool> hasConfigurationLimit()
const
294 return ::pinocchio::hasConfigurationLimit(*
this);
297 const std::vector<bool> hasConfigurationLimitInTangent()
const
299 return ::pinocchio::hasConfigurationLimitInTangent(*
this);
302 template<
typename Jo
intModelDerived>
304 : JointModelVariant((JointModelVariant)jmodel.derived())
307 (boost::mpl::contains<typename JointModelVariant::types, JointModelDerived>));
310 JointModelVariant & toVariant()
312 return *
static_cast<JointModelVariant *
>(
this);
315 const JointModelVariant & toVariant()
const
317 return *
static_cast<const JointModelVariant *
>(
this);
320 JointDataDerived createData()
const
322 return ::pinocchio::createData<Scalar, Options, JointCollectionTpl>(*
this);
325 template<
typename Jo
intModelDerived>
328 return ::pinocchio::isEqual(*
this, other.derived());
331 template<
typename Jo
intModelDerived>
334 return ::pinocchio::hasSameIndexes(*
this, other.derived());
339 return Base::isEqual(other) && toVariant() == other.toVariant();
344 return isEqual(other);
349 return !(*
this == other);
352 template<
typename ConfigVector>
353 void calc(JointDataDerived & data,
const Eigen::MatrixBase<ConfigVector> & q)
const
358 template<
typename TangentVector>
360 JointDataDerived & data,
const Blank blank,
const Eigen::MatrixBase<TangentVector> & v)
const
365 template<
typename ConfigVector,
typename TangentVector>
367 JointDataDerived & data,
368 const Eigen::MatrixBase<ConfigVector> & q,
369 const Eigen::MatrixBase<TangentVector> & v)
const
374 template<
typename VectorLike,
typename Matrix6Like>
376 JointDataDerived & data,
377 const Eigen::MatrixBase<VectorLike> & armature,
378 const Eigen::MatrixBase<Matrix6Like> & I,
379 const bool update_I)
const
381 ::pinocchio::calc_aba(
382 *
this, data, armature.derived(), PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like, I), update_I);
389 JointMappedConfigSelector_impl(
const Eigen::MatrixBase<D> & a)
const
391 typedef const Eigen::MatrixBase<D> & InputType;
393 typedef JointMappedConfigSelectorVisitor<InputType, ReturnType> Visitor;
394 typename Visitor::ArgsType arg(a);
395 return Visitor::run(*
this, arg);
401 JointMappedConfigSelector_impl(Eigen::MatrixBase<D> & a)
const
403 typedef Eigen::MatrixBase<D> & InputType;
405 typedef JointMappedConfigSelectorVisitor<InputType, ReturnType> Visitor;
406 typename Visitor::ArgsType arg(a);
407 return Visitor::run(*
this, arg);
410 std::string shortname()
const
412 return ::pinocchio::shortname(*
this);
414 static std::string classname()
421 return ::pinocchio::nq(*
this);
425 return ::pinocchio::nv(*
this);
427 int nvExtended_impl()
const
429 return ::pinocchio::nvExtended(*
this);
432 int idx_q_impl()
const
434 return ::pinocchio::idx_q(*
this);
436 int idx_v_impl()
const
438 return ::pinocchio::idx_v(*
this);
440 int idx_vExtended_impl()
const
442 return ::pinocchio::idx_vExtended(*
this);
445 JointIndex id_impl()
const
447 return ::pinocchio::id(*
this);
450 void setIndexes(JointIndex
id,
int nq,
int nv)
452 ::pinocchio::setIndexes(*
this,
id,
nq,
nv,
nv);
455 void setIndexes(JointIndex
id,
int nq,
int nv,
int nvExtended)
461 template<
typename NewScalar>
464 return cast_joint<NewScalar, Scalar, Options, JointCollectionTpl>(*
this);
468 typedef PINOCCHIO_ALIGNED_STD_VECTOR(JointData) JointDataVector;
469 typedef PINOCCHIO_ALIGNED_STD_VECTOR(JointModel) JointModelVector;
474 template<
typename S,
int O>
class JointCollectionTpl,
475 typename JointDataDerived>
477 const JointDataBase<JointDataDerived> & joint_data,
478 const JointDataTpl<Scalar, Options, JointCollectionTpl> & joint_data_generic)
480 return joint_data_generic == joint_data.derived();
486 template<
typename S,
int O>
class JointCollectionTpl,
487 typename JointDataDerived>
489 const JointDataBase<JointDataDerived> & joint_data,
490 const JointDataTpl<Scalar, Options, JointCollectionTpl> & joint_data_generic)
492 return joint_data_generic != joint_data.derived();
498 template<
typename S,
int O>
class JointCollectionTpl,
499 typename JointModelDerived>
501 const JointModelBase<JointModelDerived> & joint_model,
502 const JointModelTpl<Scalar, Options, JointCollectionTpl> & joint_model_generic)
504 return joint_model_generic == joint_model.derived();
510 template<
typename S,
int O>
class JointCollectionTpl,
511 typename JointModelDerived>
513 const JointModelBase<JointModelDerived> & joint_model,
514 const JointModelTpl<Scalar, Options, JointCollectionTpl> & joint_model_generic)
516 return joint_model_generic != joint_model.derived();
Main pinocchio namespace.
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...
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.
MotionTpl< Scalar, Options > bias(const JointDataTpl< Scalar, Options, JointCollectionTpl > &jdata)
Visit a JointDataTpl through JointBiasVisitor to get the joint bias as a dense motion.
Eigen::Matrix< Scalar, Eigen::Dynamic, Eigen::Dynamic, Options > stu_inertia(const JointDataTpl< Scalar, Options, JointCollectionTpl > &jdata)
Visit a JointDataTpl through JointStUInertiaVisitor to get St*I*S matrix of the inertia matrix decomp...
JointMotionSubspaceTpl< Eigen::Dynamic, Scalar, Options > joint_motin_subspace_xd(const JointDataTpl< Scalar, Options, JointCollectionTpl > &jdata)
Visit a JointDataVariant through JointConstraintVisitor to get the joint constraint as a dense constr...
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...
SE3Tpl< Scalar, Options > joint_transform(const JointDataTpl< Scalar, Options, JointCollectionTpl > &jdata)
Visit a JointDataTpl through JointTransformVisitor to get the joint internal transform (transform bet...
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...
int nvExtended(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointNvExtendVisitor to get the dimension of the joint extended tangent...
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...
JointDataTpl< Scalar, Options, JointCollectionTpl >::TangentVector_t joint_v(const JointDataTpl< Scalar, Options, JointCollectionTpl > &jdata)
Visit a JointDataVariant through JointConfigVisitor to get the joint velocity vector.
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...
int nv(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointNvVisitor to get the dimension of the joint tangent space.
JointDataTpl< Scalar, Options, JointCollectionTpl >::ConfigVector_t joint_q(const JointDataTpl< Scalar, Options, JointCollectionTpl > &jdata)
Visit a JointDataVariant through JointConfigVisitor to get the joint configuration vector.
int nq(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointNqVisitor to get the dimension of the joint configuration space.
Type of the cast of a class C templated by Scalar and Options, to a new NewScalar type....
bool isEqual(const JointDataBase< Derived > &other) const
 
JointModelTpl< NewScalar, Options, JointCollectionTpl > cast() const
Common traits structure to fully define base classes for CRTP.