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
72 PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived);
74 JointModelDerived & derived() {
return *
static_cast<Derived*
>(
this); }
75 const JointModelDerived & derived()
const {
return *
static_cast<const Derived*
>(
this); }
77 JointDataDerived createData()
const {
return derived().createData(); }
79 template<
typename ConfigVectorType>
80 void calc(JointDataDerived & data,
81 const Eigen::MatrixBase<ConfigVectorType> & qs)
const
83 derived().calc(data,qs.derived());
86 template<
typename ConfigVectorType,
typename TangentVectorType>
87 void calc(JointDataDerived & data,
88 const Eigen::MatrixBase<ConfigVectorType> & qs,
89 const Eigen::MatrixBase<TangentVectorType> & vs)
const
91 derived().calc(data,qs.derived(),vs.derived());
94 template<
typename Matrix6Type>
95 void calc_aba(JointDataDerived & data,
96 const Eigen::MatrixBase<Matrix6Type> & I,
97 const bool update_I =
false)
const
99 derived().calc_aba(data, PINOCCHIO_EIGEN_CONST_CAST(Matrix6Type,I), update_I);
102 int nv()
const {
return derived().nv_impl(); }
103 int nq()
const {
return derived().nq_impl(); }
106 int nv_impl()
const {
return NV; }
107 int nq_impl()
const {
return NQ; }
109 int idx_q()
const {
return derived().idx_q_impl(); }
110 int idx_v()
const {
return derived().idx_v_impl(); }
111 JointIndex id()
const {
return derived().id_impl(); }
113 int idx_q_impl()
const {
return i_q; }
114 int idx_v_impl()
const {
return i_v; }
115 JointIndex id_impl()
const {
return i_id; }
117 void setIndexes(JointIndex
id,
int q,
int v)
118 { derived().setIndexes_impl(
id, q, v); }
120 void setIndexes_impl(JointIndex
id,
int q,
int v)
121 { i_id =
id, i_q = q; i_v = v; }
123 void disp(std::ostream & os)
const
128 <<
" index: " <<
id() << endl
129 <<
" index q: " <<
idx_q() << endl
130 <<
" index v: " <<
idx_v() << endl
131 <<
" nq: " <<
nq() << endl
132 <<
" nv: " <<
nv() << endl
142 std::string shortname()
const {
return derived().shortname(); }
143 static std::string classname() {
return Derived::classname(); }
145 template<
typename NewScalar>
147 {
return derived().template cast<NewScalar>(); }
149 template <
class OtherDerived>
151 {
return derived().isEqual(other.derived()); }
153 template <
class OtherDerived>
155 {
return !(derived() == other.derived()); }
157 template <
class OtherDerived>
163 return other.id() ==
id()
164 && other.idx_q() == idx_q()
165 && other.idx_v() == idx_v();
172 jointConfigSelector(
const Eigen::MatrixBase<D> & a)
const
173 {
return derived().jointConfigSelector_impl(a); }
177 jointConfigSelector_impl(
const Eigen::MatrixBase<D> & a)
const
183 jointConfigSelector(Eigen::MatrixBase<D> & a)
const
184 {
return derived().jointConfigSelector_impl(a); }
188 jointConfigSelector_impl(Eigen::MatrixBase<D> & a)
const
195 jointVelocitySelector(
const Eigen::MatrixBase<D> & a)
const
196 {
return derived().jointVelocitySelector_impl(a.derived()); }
200 jointVelocitySelector_impl(
const Eigen::MatrixBase<D> & a)
const
206 jointVelocitySelector(Eigen::MatrixBase<D> & a)
const
207 {
return derived().jointVelocitySelector_impl(a.derived()); }
211 jointVelocitySelector_impl(Eigen::MatrixBase<D> & a)
const
218 jointCols(
const Eigen::MatrixBase<D>& A)
const
219 {
return derived().jointCols_impl(A.derived()); }
223 jointCols_impl(
const Eigen::MatrixBase<D>& A)
const
229 jointCols(Eigen::MatrixBase<D>& A)
const
230 {
return derived().jointCols_impl(A.derived()); }
234 jointCols_impl(Eigen::MatrixBase<D> & A)
const
241 jointRows(
const Eigen::MatrixBase<D> & A)
const
242 {
return derived().jointRows_impl(A.derived()); }
246 jointRows_impl(
const Eigen::MatrixBase<D> & A)
const
252 jointRows(Eigen::MatrixBase<D> & A)
const
253 {
return derived().jointRows_impl(A.derived()); }
257 jointRows_impl(Eigen::MatrixBase<D> & A)
const
265 {
return derived().jointBlock_impl(Mat.derived()); }
269 jointBlock_impl(
const Eigen::MatrixBase<D> & Mat)
const
274 typename SizeDepType<NV>::template BlockReturn<D>::Type
276 {
return derived().jointBlock_impl(Mat.derived()); }
279 typename SizeDepType<NV>::template BlockReturn<D>::Type
280 jointBlock_impl(Eigen::MatrixBase<D> & Mat)
const
281 {
return SizeDepType<NV>::block(Mat.derived(),idx_v(),idx_v(),nv(),nv()); }
289 : i_id(std::numeric_limits<JointIndex>::max()), i_q(-1), i_v(-1) {}
319 #endif // ifndef __pinocchio_multibody_joint_model_base_hpp__