6 #ifndef __pinocchio_multibody_joint_model_base_hpp__
7 #define __pinocchio_multibody_joint_model_base_hpp__
9 #include "pinocchio/multibody/joint/joint-base.hpp"
10 #include "pinocchio/multibody/joint/joint-common-operations.hpp"
12 #include "pinocchio/math/matrix-block.hpp"
16 #define PINOCCHIO_JOINT_MODEL_TYPEDEF_GENERIC(Joint, TYPENAME) \
17 typedef Eigen::DenseIndex Index; \
18 typedef TYPENAME traits<Joint>::Scalar Scalar; \
19 typedef TYPENAME traits<Joint>::JointDataDerived JointDataDerived; \
20 typedef TYPENAME traits<Joint>::JointModelDerived JointModelDerived; \
21 typedef TYPENAME traits<Joint>::Constraint_t Constraint_t; \
22 typedef TYPENAME traits<Joint>::Transformation_t Transformation_t; \
23 typedef TYPENAME traits<Joint>::Motion_t Motion_t; \
24 typedef TYPENAME traits<Joint>::Bias_t Bias_t; \
25 typedef TYPENAME traits<Joint>::U_t U_t; \
26 typedef TYPENAME traits<Joint>::D_t D_t; \
27 typedef TYPENAME traits<Joint>::UD_t UD_t; \
28 typedef TYPENAME traits<Joint>::is_mimicable_t is_mimicable_t; \
31 Options = traits<Joint>::Options, \
32 NQ = traits<Joint>::NQ, \
33 NV = traits<Joint>::NV, \
34 NVExtended = traits<Joint>::NVExtended \
36 typedef TYPENAME traits<Joint>::ConfigVector_t ConfigVector_t; \
37 typedef TYPENAME traits<Joint>::TangentVector_t TangentVector_t
41 #define PINOCCHIO_JOINT_TYPEDEF(Joint) \
42 PINOCCHIO_JOINT_MODEL_TYPEDEF_GENERIC(Joint, PINOCCHIO_EMPTY_ARG)
43 #define PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(Joint) \
44 PINOCCHIO_JOINT_MODEL_TYPEDEF_GENERIC(Joint, typename)
46 #elif (__GNUC__ == 4) && (__GNUC_MINOR__ == 4) && (__GNUC_PATCHLEVEL__ == 2)
48 #define PINOCCHIO_JOINT_TYPEDEF(Joint) \
49 PINOCCHIO_JOINT_MODEL_TYPEDEF_GENERIC(Joint, PINOCCHIO_EMPTY_ARG)
50 #define PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(Joint) \
51 PINOCCHIO_JOINT_MODEL_TYPEDEF_GENERIC(Joint, typename)
55 #define PINOCCHIO_JOINT_TYPEDEF(Joint) PINOCCHIO_JOINT_MODEL_TYPEDEF_GENERIC(Joint, typename)
56 #define PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(Joint) \
57 PINOCCHIO_JOINT_MODEL_TYPEDEF_GENERIC(Joint, typename)
61 #define PINOCCHIO_JOINT_USE_INDEXES(Joint) \
62 typedef JointModelBase<Joint> Base; \
65 using Base::idx_vExtended
67 #define PINOCCHIO_JOINT_CAST_TYPE_SPECIALIZATION(JointModelTpl) \
68 template<typename Scalar, int Options, typename NewScalar> \
69 struct CastType<NewScalar, JointModelTpl<Scalar, Options>> \
71 typedef JointModelTpl<NewScalar, Options> type; \
77 template<
typename Derived>
80 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
83 PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived);
85 JointModelDerived & derived()
87 return *
static_cast<Derived *
>(
this);
89 const JointModelDerived & derived()
const
91 return *
static_cast<const Derived *
>(
this);
94 JointDataDerived createData()
const
96 return derived().createData();
99 const std::vector<bool> hasConfigurationLimit()
const
101 return derived().hasConfigurationLimit();
104 const std::vector<bool> hasConfigurationLimitInTangent()
const
106 return derived().hasConfigurationLimitInTangent();
109 template<
typename ConfigVectorType>
110 void calc(JointDataDerived & data,
const Eigen::MatrixBase<ConfigVectorType> & qs)
const
112 derived().calc(data, qs.derived());
115 template<
typename TangentVectorType>
117 JointDataDerived & data,
118 const Blank not_used,
119 const Eigen::MatrixBase<TangentVectorType> & vs)
const
121 derived().calc(data, not_used, vs.derived());
124 template<
typename ConfigVectorType,
typename TangentVectorType>
126 JointDataDerived & data,
127 const Eigen::MatrixBase<ConfigVectorType> & qs,
128 const Eigen::MatrixBase<TangentVectorType> & vs)
const
130 derived().calc(data, qs.derived(), vs.derived());
133 template<
typename VectorLike,
typename Matrix6Like>
135 JointDataDerived & data,
136 const Eigen::MatrixBase<VectorLike> & armature,
137 const Eigen::MatrixBase<Matrix6Like> & I,
138 const bool update_I =
false)
const
141 data, armature.derived(), PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like, I), update_I);
146 return derived().nv_impl();
150 return derived().nq_impl();
152 int nvExtended()
const
154 return derived().nvExtended_impl();
166 int nvExtended_impl()
const
173 return derived().idx_q_impl();
177 return derived().idx_v_impl();
179 int idx_vExtended()
const
181 return derived().idx_vExtended_impl();
183 JointIndex id()
const
185 return derived().id_impl();
188 int idx_q_impl()
const
192 int idx_v_impl()
const
196 int idx_vExtended_impl()
const
200 JointIndex id_impl()
const
205 void setIndexes(JointIndex
id,
int q,
int v)
207 derived().setIndexes_impl(
id, q, v, v);
210 void setIndexes(JointIndex
id,
int q,
int v,
int vExtended)
212 derived().setIndexes_impl(
id, q, v, vExtended);
215 void setIndexes_impl(JointIndex
id,
int q,
int v,
int vExtended)
219 i_vExtended = vExtended;
222 void disp(std::ostream & os)
const
226 <<
" index: " <<
id() << endl
227 <<
" index q: " <<
idx_q() << endl
228 <<
" index v: " <<
idx_v() << endl
230 <<
" nq: " <<
nq() << endl
231 <<
" nv: " <<
nv() << endl
237 jmodel.derived().disp(os);
241 std::string shortname()
const
243 return derived().shortname();
245 static std::string classname()
247 return Derived::classname();
250 template<
typename NewScalar>
253 return derived().template cast<NewScalar>();
256 template<
class OtherDerived>
259 return derived().isEqual(other.derived());
262 template<
class OtherDerived>
265 return !(internal::comparison_eq(derived(), other.derived()));
268 template<
class OtherDerived>
276 return derived().hasSameIndexes(other.derived());
279 template<
class OtherDerived>
282 return internal::comparison_eq(other.id(),
id())
283 && internal::comparison_eq(other.idx_q(), idx_q())
284 && internal::comparison_eq(other.idx_v(), idx_v())
285 && internal::comparison_eq(other.idx_vExtended(), idx_vExtended());
292 JointMappedConfigSelector(
const Eigen::MatrixBase<D> & a)
const
294 return derived().JointMappedConfigSelector_impl(a);
299 JointMappedConfigSelector_impl(
const Eigen::MatrixBase<D> & a)
const
307 JointMappedConfigSelector(Eigen::MatrixBase<D> & a)
const
309 return derived().JointMappedConfigSelector_impl(a);
314 JointMappedConfigSelector_impl(Eigen::MatrixBase<D> & a)
const
322 jointConfigSelector(
const Eigen::MatrixBase<D> & a)
const
324 return derived().jointConfigSelector_impl(a);
329 jointConfigSelector_impl(
const Eigen::MatrixBase<D> & a)
const
337 jointConfigSelector(Eigen::MatrixBase<D> & a)
const
339 return derived().jointConfigSelector_impl(a);
344 jointConfigSelector_impl(Eigen::MatrixBase<D> & a)
const
353 JointMappedVelocitySelector(
const Eigen::MatrixBase<D> & a)
const
355 return derived().JointMappedVelocitySelector_impl(a.derived());
360 JointMappedVelocitySelector_impl(
const Eigen::MatrixBase<D> & a)
const
368 JointMappedVelocitySelector(Eigen::MatrixBase<D> & a)
const
370 return derived().JointMappedVelocitySelector_impl(a.derived());
375 JointMappedVelocitySelector_impl(Eigen::MatrixBase<D> & a)
const
383 jointVelocitySelector(
const Eigen::MatrixBase<D> & a)
const
385 return derived().jointVelocitySelector_impl(a.derived());
390 jointVelocitySelector_impl(
const Eigen::MatrixBase<D> & a)
const
398 jointVelocitySelector(Eigen::MatrixBase<D> & a)
const
400 return derived().jointVelocitySelector_impl(a.derived());
405 jointVelocitySelector_impl(Eigen::MatrixBase<D> & a)
const
414 jointCols(
const Eigen::MatrixBase<D> & A)
const
416 return derived().jointCols_impl(A.derived());
421 jointExtendedModelCols(
const Eigen::MatrixBase<D> & A)
const
423 return derived().jointExtendedModelCols_impl(A.derived());
428 jointCols_impl(
const Eigen::MatrixBase<D> & A)
const
435 jointExtendedModelCols_impl(
const Eigen::MatrixBase<D> & A)
const
445 return derived().jointCols_impl(A.derived());
450 jointExtendedModelCols(Eigen::MatrixBase<D> & A)
const
452 return derived().jointExtendedModelCols_impl(A.derived());
457 jointCols_impl(Eigen::MatrixBase<D> & A)
const
464 jointExtendedModelCols_impl(Eigen::MatrixBase<D> & A)
const
473 jointRows(
const Eigen::MatrixBase<D> & A)
const
475 return derived().jointRows_impl(A.derived());
480 jointExtendedModelRows(
const Eigen::MatrixBase<D> & A)
const
482 return derived().jointExtendedModelRows_impl(A.derived());
487 jointRows_impl(
const Eigen::MatrixBase<D> & A)
const
494 jointExtendedModelRows_impl(
const Eigen::MatrixBase<D> & A)
const
503 return derived().jointRows_impl(A.derived());
508 jointExtendedModelRows(Eigen::MatrixBase<D> & A)
const
510 return derived().jointExtendedModelRows_impl(A.derived());
515 jointRows_impl(Eigen::MatrixBase<D> & A)
const
522 jointExtendedModelRows_impl(Eigen::MatrixBase<D> & A)
const
534 return derived().jointBlock_impl(Mat.derived());
539 jointExtendedModelBlock(
const Eigen::MatrixBase<D> & Mat)
const
541 return derived().jointExtendedModelBlock_impl(Mat.derived());
545 typename SizeDepType<NV>::template BlockReturn<D>::ConstType
546 jointBlock_impl(
const Eigen::MatrixBase<D> & Mat)
const
548 return SizeDepType<NV>::block(Mat.derived(), idx_v(), idx_v(), nv(), nv());
552 typename SizeDepType<NVExtended>::template BlockReturn<D>::ConstType
553 jointExtendedModelBlock_impl(
const Eigen::MatrixBase<D> & Mat)
const
555 return SizeDepType<NVExtended>::block(
556 Mat.derived(), idx_vExtended(), idx_vExtended(), nvExtended(), nvExtended());
561 typename SizeDepType<NV>::template BlockReturn<D>::Type
564 return derived().jointBlock_impl(Mat.derived());
568 typename SizeDepType<NVExtended>::template BlockReturn<D>::Type
569 jointExtendedModelBlock(Eigen::MatrixBase<D> & Mat)
const
571 return derived().jointExtendedModelBlock_impl(Mat.derived());
575 typename SizeDepType<NV>::template BlockReturn<D>::Type
576 jointBlock_impl(Eigen::MatrixBase<D> & Mat)
const
578 return SizeDepType<NV>::block(Mat.derived(), idx_v(), idx_v(), nv(), nv());
582 typename SizeDepType<NVExtended>::template BlockReturn<D>::Type
583 jointExtendedModelBlock_impl(Eigen::MatrixBase<D> & Mat)
const
585 return SizeDepType<NVExtended>::block(
586 Mat.derived(), idx_vExtended(), idx_vExtended(), nvExtended(), nvExtended());
594 : i_id(std::numeric_limits<JointIndex>::max())
619 i_vExtended = clone.i_vExtended;
Main pinocchio namespace.
JointIndex id(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointIdVisitor to get the index of the joint in the kinematic chain.
int nvExtended(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointNvExtendVisitor to get the dimension of the joint extended tangent...
int idx_vExtended(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointIdvExtendedVisitor to get the index in the model extended tangent ...
int idx_v(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointIdxVVisitor to get the index in the model tangent space correspond...
int nv(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointNvVisitor to get the dimension of the joint tangent space.
int nq(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointNqVisitor to get the dimension of the joint configuration space.
std::string shortname(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointShortnameVisitor to get the shortname of the derived joint model.
int idx_q(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointIdxQVisitor to get the index in the full model configuration space...
Type of the cast of a class C templated by Scalar and Options, to a new NewScalar type....
JointModelBase(const JointModelBase &clone)
SizeDepType< NV >::template BlockReturn< D >::ConstType jointBlock(const Eigen::MatrixBase< D > &Mat) const
Returns a block of dimension nv()xnv() located at position idx_v(),idx_v() in the matrix Mat.
JointModelBase & operator=(const JointModelBase &clone)
Common traits structure to fully define base classes for CRTP.