6 #ifndef __pinocchio_joint_base_hpp__ 7 #define __pinocchio_joint_base_hpp__ 9 #include "pinocchio/math/matrix-block.hpp" 11 #include "pinocchio/multibody/fwd.hpp" 12 #include "pinocchio/multibody/joint/fwd.hpp" 13 #include "pinocchio/multibody/joint/joint-common-operations.hpp" 17 #define PINOCCHIO_JOINT_TYPEDEF_GENERIC(Joint,TYPENAME) \ 18 typedef Eigen::DenseIndex Index; \ 19 typedef TYPENAME traits<Joint>::Scalar Scalar; \ 20 typedef TYPENAME traits<Joint>::JointDataDerived JointDataDerived; \ 21 typedef TYPENAME traits<Joint>::JointModelDerived JointModelDerived; \ 22 typedef TYPENAME traits<Joint>::Constraint_t Constraint_t; \ 23 typedef TYPENAME traits<Joint>::Transformation_t Transformation_t; \ 24 typedef TYPENAME traits<Joint>::Motion_t Motion_t; \ 25 typedef TYPENAME traits<Joint>::Bias_t Bias_t; \ 26 typedef TYPENAME traits<Joint>::U_t U_t; \ 27 typedef TYPENAME traits<Joint>::D_t D_t; \ 28 typedef TYPENAME traits<Joint>::UD_t UD_t; \ 30 Options = traits<Joint>::Options, \ 31 NQ = traits<Joint>::NQ, \ 32 NV = traits<Joint>::NV \ 34 typedef TYPENAME traits<Joint>::ConfigVector_t ConfigVector_t; \ 35 typedef TYPENAME traits<Joint>::TangentVector_t TangentVector_t 37 #define PINOCCHIO_JOINT_DATA_TYPEDEF_GENERIC(Joint,TYPENAME) \ 38 PINOCCHIO_JOINT_TYPEDEF_GENERIC(Joint,TYPENAME); \ 39 typedef TYPENAME traits<Joint>::ConstraintTypeConstRef ConstraintTypeConstRef; \ 40 typedef TYPENAME traits<Joint>::TansformTypeConstRef TansformTypeConstRef; \ 41 typedef TYPENAME traits<Joint>::MotionTypeConstRef MotionTypeConstRef; \ 42 typedef TYPENAME traits<Joint>::BiasTypeConstRef BiasTypeConstRef; \ 43 typedef TYPENAME traits<Joint>::UTypeConstRef UTypeConstRef; \ 44 typedef TYPENAME traits<Joint>::UTypeRef UTypeRef; \ 45 typedef TYPENAME traits<Joint>::DTypeConstRef DTypeConstRef; \ 46 typedef TYPENAME traits<Joint>::UDTypeConstRef UDTypeConstRef 50 #define PINOCCHIO_JOINT_TYPEDEF(Joint) PINOCCHIO_JOINT_TYPEDEF_GENERIC(Joint,PINOCCHIO_EMPTY_ARG) 51 #define PINOCCHIO_JOINT_DATA_TYPEDEF(Joint) PINOCCHIO_JOINT_DATA_TYPEDEF_GENERIC(Joint,PINOCCHIO_EMPTY_ARG) 53 #define PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(Joint) PINOCCHIO_JOINT_TYPEDEF_GENERIC(Joint,typename) 54 #define PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(Joint) PINOCCHIO_JOINT_DATA_TYPEDEF_GENERIC(Joint,typename) 56 #elif (__GNUC__ == 4) && (__GNUC_MINOR__ == 4) && (__GNUC_PATCHLEVEL__ == 2) 58 #define PINOCCHIO_JOINT_TYPEDEF(Joint) PINOCCHIO_JOINT_TYPEDEF_GENERIC(Joint,PINOCCHIO_EMPTY_ARG) 59 #define PINOCCHIO_JOINT_DATA_TYPEDEF(Joint) PINOCCHIO_JOINT_DATA_TYPEDEF_GENERIC(Joint,PINOCCHIO_EMPTY_ARG) 61 #define PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(Joint) PINOCCHIO_JOINT_TYPEDEF_GENERIC(Joint,typename) 62 #define PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(Joint) PINOCCHIO_JOINT_DATA_TYPEDEF_GENERIC(Joint,typename) 66 #define PINOCCHIO_JOINT_TYPEDEF(Joint) PINOCCHIO_JOINT_TYPEDEF_GENERIC(Joint,typename) 67 #define PINOCCHIO_JOINT_DATA_TYPEDEF(Joint) PINOCCHIO_JOINT_DATA_TYPEDEF_GENERIC(Joint,typename) 69 #define PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(Joint) PINOCCHIO_JOINT_TYPEDEF_GENERIC(Joint,typename) 70 #define PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(Joint) PINOCCHIO_JOINT_DATA_TYPEDEF_GENERIC(Joint,typename) 74 #define PINOCCHIO_JOINT_USE_INDEXES \ 75 typedef JointModelBase<JointModelDerived> Base; \ 79 #define PINOCCHIO_JOINT_CAST_TYPE_SPECIALIZATION(JointModelTpl) \ 80 template<typename Scalar, int Options, typename NewScalar> \ 81 struct CastType< NewScalar, JointModelTpl<Scalar,Options> > \ 82 { typedef JointModelTpl<NewScalar,Options> type; } 85 #define PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR \ 86 ConstraintTypeConstRef S_accessor() const { return S; } \ 87 TansformTypeConstRef M_accessor() const { return M; } \ 88 MotionTypeConstRef v_accessor() const { return v; } \ 89 BiasTypeConstRef c_accessor() const { return c; } \ 90 UTypeConstRef U_accessor() const { return U; } \ 91 UTypeRef U_accessor() { return U; } \ 92 DTypeConstRef Dinv_accessor() const { return Dinv; } \ 93 UDTypeConstRef UDinv_accessor() const { return UDinv; } 95 #define PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE \ 96 typedef const Constraint_t & ConstraintTypeConstRef; \ 97 typedef const Transformation_t & TansformTypeConstRef; \ 98 typedef const Motion_t & MotionTypeConstRef; \ 99 typedef const Bias_t & BiasTypeConstRef; \ 100 typedef const U_t & UTypeConstRef; \ 101 typedef U_t & UTypeRef; \ 102 typedef const D_t & DTypeConstRef; \ 103 typedef const UD_t & UDTypeConstRef; 108 template<
typename Derived>
111 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
114 PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(JointDerived);
116 Derived & derived() {
return *
static_cast<Derived*
>(
this); }
117 const Derived & derived()
const {
return *
static_cast<const Derived*
>(
this); }
119 ConstraintTypeConstRef S()
const {
return derived().S_accessor(); }
120 TansformTypeConstRef M()
const {
return derived().M_accessor(); }
121 MotionTypeConstRef v()
const {
return derived().v_accessor(); }
122 BiasTypeConstRef c()
const {
return derived().c_accessor(); }
124 UTypeConstRef U()
const {
return derived().U_accessor(); }
125 UTypeRef U() {
return derived().U_accessor(); }
126 DTypeConstRef Dinv()
const {
return derived().Dinv_accessor(); }
127 UDTypeConstRef UDinv()
const {
return derived().UDinv_accessor(); }
129 std::string shortname()
const {
return derived().shortname(); }
130 static std::string classname() {
return Derived::classname(); }
132 void disp(std::ostream & os)
const 138 friend std::ostream & operator << (std::ostream & os, const JointDataBase<Derived> & joint)
151 template<
typename Derived>
154 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
157 PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived);
159 JointModelDerived & derived() {
return *
static_cast<Derived*
>(
this); }
160 const JointModelDerived & derived()
const {
return *
static_cast<const Derived*
>(
this); }
162 JointDataDerived
createData()
const {
return derived().createData(); }
164 template<
typename ConfigVectorType>
165 void calc(JointDataDerived & data,
166 const Eigen::MatrixBase<ConfigVectorType> & qs)
const 168 derived().calc(data,qs.derived());
171 template<
typename ConfigVectorType,
typename TangentVectorType>
172 void calc(JointDataDerived & data,
173 const Eigen::MatrixBase<ConfigVectorType> & qs,
174 const Eigen::MatrixBase<TangentVectorType> & vs)
const 176 derived().calc(data,qs.derived(),vs.derived());
179 template<
typename Matrix6Type>
180 void calc_aba(JointDataDerived & data,
181 const Eigen::MatrixBase<Matrix6Type> & I,
182 const bool update_I =
false)
const 184 derived().calc_aba(data, PINOCCHIO_EIGEN_CONST_CAST(Matrix6Type,I), update_I);
187 int nv()
const {
return derived().nv_impl(); }
188 int nq()
const {
return derived().nq_impl(); }
191 int nv_impl()
const {
return NV; }
192 int nq_impl()
const {
return NQ; }
194 int idx_q()
const {
return derived().idx_q_impl(); }
195 int idx_v()
const {
return derived().idx_v_impl(); }
196 JointIndex
id()
const {
return derived().id_impl(); }
198 int idx_q_impl()
const {
return i_q; }
199 int idx_v_impl()
const {
return i_v; }
200 JointIndex id_impl()
const {
return i_id; }
203 { derived().setIndexes_impl(
id, q, v); }
205 void setIndexes_impl(JointIndex
id,
int q,
int v)
206 { i_id =
id, i_q = q; i_v = v; }
208 void disp(std::ostream & os)
const 213 <<
" index: " <<
id() << endl
214 <<
" index q: " <<
idx_q() << endl
215 <<
" index v: " <<
idx_v() << endl
216 <<
" nq: " <<
nq() << endl
217 <<
" nv: " <<
nv() << endl
221 friend std::ostream & operator << (std::ostream & os, const JointModelBase<Derived> & joint)
227 std::string
shortname()
const {
return derived().shortname(); }
228 static std::string classname() {
return Derived::classname(); }
230 template<
typename NewScalar>
232 {
return derived().template cast<NewScalar>(); }
234 template <
class OtherDerived>
236 {
return derived().isEqual(other.derived()); }
238 template <
class OtherDerived>
240 {
return !(derived() == other.derived()); }
242 template <
class OtherDerived>
248 return other.id() ==
id()
249 && other.idx_q() ==
idx_q()
250 && other.idx_v() ==
idx_v();
257 jointConfigSelector(
const Eigen::MatrixBase<D> & a)
const 258 {
return derived().jointConfigSelector_impl(a); }
262 jointConfigSelector_impl(
const Eigen::MatrixBase<D> & a)
const 268 jointConfigSelector(Eigen::MatrixBase<D> & a)
const 269 {
return derived().jointConfigSelector_impl(a); }
273 jointConfigSelector_impl(Eigen::MatrixBase<D> & a)
const 280 jointVelocitySelector(
const Eigen::MatrixBase<D> & a)
const 281 {
return derived().jointVelocitySelector_impl(a.derived()); }
285 jointVelocitySelector_impl(
const Eigen::MatrixBase<D> & a)
const 291 jointVelocitySelector(Eigen::MatrixBase<D> & a)
const 292 {
return derived().jointVelocitySelector_impl(a.derived()); }
296 jointVelocitySelector_impl(Eigen::MatrixBase<D> & a)
const 303 jointCols(
const Eigen::MatrixBase<D>& A)
const 304 {
return derived().jointCols_impl(A.derived()); }
308 jointCols_impl(
const Eigen::MatrixBase<D>& A)
const 314 jointCols(Eigen::MatrixBase<D>& A)
const 315 {
return derived().jointCols_impl(A.derived()); }
319 jointCols_impl(Eigen::MatrixBase<D> & A)
const 326 jointRows(
const Eigen::MatrixBase<D> & A)
const 327 {
return derived().jointRows_impl(A.derived()); }
331 jointRows_impl(
const Eigen::MatrixBase<D> & A)
const 337 jointRows(Eigen::MatrixBase<D> & A)
const 338 {
return derived().jointRows_impl(A.derived()); }
342 jointRows_impl(Eigen::MatrixBase<D> & A)
const 350 {
return derived().jointBlock_impl(Mat.derived()); }
354 jointBlock_impl(
const Eigen::MatrixBase<D> & Mat)
const 360 jointBlock(Eigen::MatrixBase<D> & Mat)
const 361 {
return derived().jointBlock_impl(Mat.derived()); }
365 jointBlock_impl(Eigen::MatrixBase<D> & Mat)
const 374 : i_id(
std::numeric_limits<JointIndex>::max()), i_q(-1), i_v(-1) {}
404 #endif // ifndef __pinocchio_joint_base_hpp__ int nv(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointNvVisitor to get the dimension of the joint tangent space...
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...
int idx_q(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointIdxQVisitor to get the index in the full model configuration space...
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...
JointDataTpl< Scalar, Options, JointCollectionTpl > createData(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through CreateData visitor to create a JointDataTpl.
JointIndex id(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointIdVisitor to get the index of the joint in the kinematic chain...
JointDataBase()
Default constructor: protected.
std::string shortname(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointShortnameVisitor to get the shortname of the derived joint model...
JointModelBase & operator=(const JointModelBase &clone)
Main pinocchio namespace.
JointModelBase(const JointModelBase &clone)
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...
Common traits structure to fully define base classes for CRTP.
Type of the cast of a class C templated by Scalar and Options, to a new NewScalar type...
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...