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; \ 29 Options = traits<Joint>::Options, \ 30 NQ = traits<Joint>::NQ, \ 31 NV = traits<Joint>::NV \ 33 typedef TYPENAME traits<Joint>::ConfigVector_t ConfigVector_t; \ 34 typedef TYPENAME traits<Joint>::TangentVector_t TangentVector_t 38 #define PINOCCHIO_JOINT_TYPEDEF(Joint) PINOCCHIO_JOINT_MODEL_TYPEDEF_GENERIC(Joint,PINOCCHIO_EMPTY_ARG) 39 #define PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(Joint) PINOCCHIO_JOINT_MODEL_TYPEDEF_GENERIC(Joint,typename) 41 #elif (__GNUC__ == 4) && (__GNUC_MINOR__ == 4) && (__GNUC_PATCHLEVEL__ == 2) 43 #define PINOCCHIO_JOINT_TYPEDEF(Joint) PINOCCHIO_JOINT_MODEL_TYPEDEF_GENERIC(Joint,PINOCCHIO_EMPTY_ARG) 44 #define PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(Joint) PINOCCHIO_JOINT_MODEL_TYPEDEF_GENERIC(Joint,typename) 48 #define PINOCCHIO_JOINT_TYPEDEF(Joint) PINOCCHIO_JOINT_MODEL_TYPEDEF_GENERIC(Joint,typename) 49 #define PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(Joint) PINOCCHIO_JOINT_MODEL_TYPEDEF_GENERIC(Joint,typename) 53 #define PINOCCHIO_JOINT_USE_INDEXES(Joint) \ 54 typedef JointModelBase<Joint> Base; \ 58 #define PINOCCHIO_JOINT_CAST_TYPE_SPECIALIZATION(JointModelTpl) \ 59 template<typename Scalar, int Options, typename NewScalar> \ 60 struct CastType< NewScalar, JointModelTpl<Scalar,Options> > \ 61 { typedef JointModelTpl<NewScalar,Options> type; } 66 template<
typename Derived>
69 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
74 JointModelDerived &
derived() {
return *
static_cast<Derived*
>(
this); }
75 const JointModelDerived &
derived()
const {
return *
static_cast<const Derived*
>(
this); }
81 return derived().hasConfigurationLimit();
86 return derived().hasConfigurationLimitInTangent();
89 template<
typename ConfigVectorType>
91 const Eigen::MatrixBase<ConfigVectorType> &
qs)
const 93 derived().calc(data,qs.derived());
96 template<
typename ConfigVectorType,
typename TangentVectorType>
98 const Eigen::MatrixBase<ConfigVectorType> &
qs,
99 const Eigen::MatrixBase<TangentVectorType> & vs)
const 101 derived().calc(data,qs.derived(),vs.derived());
104 template<
typename Matrix6Type>
106 const Eigen::MatrixBase<Matrix6Type> & I,
107 const bool update_I =
false)
const 128 {
derived().setIndexes_impl(
id, q, v); }
133 void disp(std::ostream & os)
const 138 <<
" index: " <<
id() << endl
139 <<
" index q: " <<
idx_q() << endl
140 <<
" index v: " <<
idx_v() << endl
141 <<
" nq: " <<
nq() << endl
142 <<
" nv: " <<
nv() << endl
146 friend std::ostream & operator << (std::ostream & os, const JointModelBase<Derived> & joint)
153 static std::string
classname() {
return Derived::classname(); }
155 template<
typename NewScalar>
157 {
return derived().template cast<NewScalar>(); }
159 template <
class OtherDerived>
163 template <
class OtherDerived>
167 template <
class OtherDerived>
176 template <
class OtherDerived>
179 return other.
id() ==
id()
189 {
return derived().jointConfigSelector_impl(a); }
200 {
return derived().jointConfigSelector_impl(a); }
212 {
return derived().jointVelocitySelector_impl(a.derived()); }
223 {
return derived().jointVelocitySelector_impl(a.derived()); }
235 {
return derived().jointCols_impl(A.derived()); }
246 {
return derived().jointCols_impl(A.derived()); }
258 {
return derived().jointRows_impl(A.derived()); }
269 {
return derived().jointRows_impl(A.derived()); }
281 {
return derived().jointBlock_impl(Mat.derived()); }
292 {
return derived().jointBlock_impl(Mat.derived()); }
335 #endif // ifndef __pinocchio_multibody_joint_model_base_hpp__ JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > const Eigen::MatrixBase< TangentVectorType > & v
SizeDepType< NV >::template SegmentReturn< D >::Type jointVelocitySelector(Eigen::MatrixBase< D > &a) const
JointIndex id_impl() const
bool operator!=(const JointModelBase< OtherDerived > &other) const
SizeDepType< NV >::template ColsReturn< D >::Type jointCols(Eigen::MatrixBase< D > &A) const
void setIndexes(JointIndex id, int q, int v)
virtual CollisionGeometry * clone() const=0
SizeDepType< NV >::template SegmentReturn< D >::ConstType jointVelocitySelector_impl(const Eigen::MatrixBase< D > &a) const
SizeDepType< NQ >::template SegmentReturn< D >::Type jointConfigSelector(Eigen::MatrixBase< D > &a) const
SizeDepType< NV >::template BlockReturn< D >::Type jointBlock(Eigen::MatrixBase< D > &Mat) const
SizeDepType< NV >::template SegmentReturn< D >::ConstType jointVelocitySelector(const Eigen::MatrixBase< D > &a) const
std::string shortname() const
JointModelBase & operator=(const JointModelBase &clone)
AD< Scalar > max(const AD< Scalar > &x, const AD< Scalar > &y)
CastType< NewScalar, Derived >::type cast() const
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...
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > & q
const std::vector< bool > hasConfigurationLimit() const
#define PINOCCHIO_EIGEN_CONST_CAST(TYPE, OBJ)
Macro for an automatic const_cast.
void setIndexes_impl(JointIndex id, int q, int v)
JointDataDerived createData() const
bool hasSameIndexes(const JointModelBase< OtherDerived > &other) const
static BlockReturn< D >::ConstType block(const Eigen::MatrixBase< D > &mat, typename Eigen::DenseBase< D >::Index row_id, typename Eigen::DenseBase< D >::Index col_id, typename Eigen::DenseBase< D >::Index row_size_block=NV, typename Eigen::DenseBase< D >::Index col_size_block=NV)
bool operator==(const JointModelBase< OtherDerived > &other) const
SizeDepType< NV >::template RowsReturn< D >::ConstType jointRows_impl(const Eigen::MatrixBase< D > &A) const
const std::vector< bool > hasConfigurationLimitInTangent() const
SizeDepType< NV >::template RowsReturn< D >::ConstType jointRows(const Eigen::MatrixBase< D > &A) const
const JointModelDerived & derived() const
SizeDepType< NV >::template SegmentReturn< D >::Type jointVelocitySelector_impl(Eigen::MatrixBase< D > &a) const
SizeDepType< NQ >::template SegmentReturn< D >::ConstType jointConfigSelector(const Eigen::MatrixBase< D > &a) const
SizeDepType< NV >::template RowsReturn< D >::Type jointRows(Eigen::MatrixBase< D > &A) const
SizeDepType< NV >::template BlockReturn< D >::ConstType jointBlock_impl(const Eigen::MatrixBase< D > &Mat) const
SizeDepType< NV >::template ColsReturn< D >::Type jointCols_impl(Eigen::MatrixBase< D > &A) const
SizeDepType< NQ >::template SegmentReturn< D >::ConstType jointConfigSelector_impl(const Eigen::MatrixBase< D > &a) const
bool isEqual(const JointModelBase< OtherDerived > &) const
SizeDepType< NV >::template BlockReturn< D >::Type jointBlock_impl(Eigen::MatrixBase< D > &Mat) const
JointModelDerived & derived()
void calc(JointDataDerived &data, const Eigen::MatrixBase< ConfigVectorType > &qs) const
SizeDepType< NV >::template ColsReturn< D >::ConstType jointCols_impl(const Eigen::MatrixBase< D > &A) const
void calc_aba(JointDataDerived &data, const Eigen::MatrixBase< Matrix6Type > &I, const bool update_I=false) const
static ColsReturn< D >::ConstType middleCols(const Eigen::MatrixBase< D > &mat, typename Eigen::DenseBase< D >::Index start, typename Eigen::DenseBase< D >::Index size=NV)
static std::string classname()
Main pinocchio namespace.
void disp(std::ostream &os) const
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef traits< Derived >::JointDerived JointDerived
PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived)
JointModelBase(const JointModelBase &clone)
Common traits structure to fully define base classes for CRTP.
SizeDepType< NV >::template RowsReturn< D >::Type jointRows_impl(Eigen::MatrixBase< D > &A) const
SizeDepType< NV >::template ColsReturn< D >::ConstType jointCols(const Eigen::MatrixBase< D > &A) const
static RowsReturn< D >::ConstType middleRows(const Eigen::MatrixBase< D > &mat, typename Eigen::DenseBase< D >::Index start, typename Eigen::DenseBase< D >::Index size=NV)
static SegmentReturn< D >::ConstType segment(const Eigen::MatrixBase< D > &mat, typename Eigen::DenseBase< D >::Index start, typename Eigen::DenseBase< D >::Index size=NV)
Type of the cast of a class C templated by Scalar and Options, to a new NewScalar type...
bool isEqual(const JointModelBase< Derived > &other) const
void calc(JointDataDerived &data, const Eigen::MatrixBase< ConfigVectorType > &qs, const Eigen::MatrixBase< TangentVectorType > &vs) const
SizeDepType< NQ >::template SegmentReturn< D >::Type jointConfigSelector_impl(Eigen::MatrixBase< D > &a) const