5 #ifndef __pinocchio_joint_generic_hpp__
6 #define __pinocchio_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>
18 template<
typename Scalar,
int Options = 0,
template<
typename S,
int O>
class JointCollectionTpl = JointCollectionDefaultTpl>
22 template<
typename _Scalar,
int _Options,
template<
typename S,
int O>
class JointCollectionTpl>
31 typedef _Scalar Scalar;
32 typedef JointCollectionTpl<Scalar,Options> JointCollection;
42 typedef Eigen::Matrix<Scalar,6,Eigen::Dynamic,Options> U_t;
43 typedef Eigen::Matrix<Scalar,Eigen::Dynamic,Eigen::Dynamic,Options> D_t;
44 typedef Eigen::Matrix<Scalar,6,Eigen::Dynamic,Options> UD_t;
54 typedef U_t UTypeConstRef;
56 typedef D_t DTypeConstRef;
58 typedef UD_t UDTypeConstRef;
59 typedef UD_t UDTypeRef;
61 typedef Eigen::Matrix<Scalar,Eigen::Dynamic,1,Options> ConfigVector_t;
62 typedef Eigen::Matrix<Scalar,Eigen::Dynamic,1,Options> TangentVector_t;
65 template<
typename Scalar,
int Options,
template<
typename S,
int O>
class JointCollectionTpl>
69 template<
typename Scalar,
int Options,
template<
typename S,
int O>
class JointCollectionTpl>
73 template<
typename _Scalar,
int _Options,
template<
typename S,
int O>
class JointCollectionTpl>
75 :
public JointDataBase< JointDataTpl<_Scalar,_Options,JointCollectionTpl> >
76 , JointCollectionTpl<_Scalar,_Options>::JointDataVariant
78 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
83 PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(JointDerived);
85 typedef JointCollectionTpl<_Scalar,_Options> JointCollection;
86 typedef typename JointCollection::JointDataVariant JointDataVariant;
88 using Base::operator==;
89 using Base::operator!=;
91 JointDataVariant & toVariant() {
return *
static_cast<JointDataVariant*
>(
this); }
92 const JointDataVariant & toVariant()
const {
return *
static_cast<const JointDataVariant*
>(
this); }
96 Motion_t v()
const {
return motion(*
this); }
97 Bias_t c()
const {
return bias(*
this); }
100 U_t U()
const {
return u_inertia(*
this); }
108 JointDataTpl(
const JointDataVariant & jdata_variant)
109 : JointDataVariant(jdata_variant)
112 template<
typename Jo
intDataDerived>
113 JointDataTpl(
const JointDataBase<JointDataDerived> & jdata)
114 : JointCollection::JointDataVariant((JointDataVariant)jdata.derived())
116 BOOST_MPL_ASSERT((boost::mpl::contains<typename JointDataVariant::types,JointDataDerived>));
120 Constraint_t S_accessor()
const {
return S(); }
121 Transformation_t M_accessor()
const {
return M(); }
122 Motion_t v_accessor()
const {
return v(); }
123 Bias_t c_accessor()
const {
return c(); }
124 U_t U_accessor()
const {
return U(); }
125 D_t Dinv_accessor()
const {
return Dinv(); }
126 UD_t UDinv_accessor()
const {
return UDinv(); }
128 static std::string classname() {
return "JointData"; }
129 std::string shortname()
const { return ::pinocchio::shortname(*
this); }
131 bool isEqual(
const JointDataTpl & other)
const
134 && toVariant() == other.toVariant();
139 template<
typename NewScalar,
typename Scalar,
int Options,
template<
typename S,
int O>
class JointCollectionTpl>
145 template<
typename _Scalar,
int _Options,
template<
typename S,
int O>
class JointCollectionTpl>
147 :
JointModelBase< JointModelTpl<_Scalar,_Options,JointCollectionTpl> >
148 , JointCollectionTpl<_Scalar,_Options>::JointModelVariant
150 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
154 PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived);
157 typedef JointCollectionTpl<Scalar,Options> JointCollection;
158 typedef typename JointCollection::JointDataVariant JointDataVariant;
159 typedef typename JointCollection::JointModelVariant JointModelVariant;
162 using Base::setIndexes;
163 using Base::operator==;
164 using Base::operator!=;
168 JointModelTpl(
const JointModelVariant & jmodel_variant)
169 : JointCollection::JointModelVariant(jmodel_variant)
172 template<
typename Jo
intModelDerived>
173 JointModelTpl(
const JointModelBase<JointModelDerived> & jmodel)
174 : JointModelVariant((JointModelVariant)jmodel.derived())
176 BOOST_MPL_ASSERT((boost::mpl::contains<typename JointModelVariant::types,JointModelDerived>));
179 JointModelVariant & toVariant()
180 {
return *
static_cast<JointModelVariant*
>(
this); }
182 const JointModelVariant & toVariant()
const
183 {
return *
static_cast<const JointModelVariant*
>(
this); }
185 JointDataDerived createData()
const
186 { return ::pinocchio::createData<Scalar,Options,JointCollectionTpl>(*
this); }
189 bool isEqual(
const JointModelTpl & other)
const
191 return Base::isEqual(other)
192 && toVariant() == other.toVariant();
195 template<
typename ConfigVector>
196 void calc(JointDataDerived & data,
197 const Eigen::MatrixBase<ConfigVector> & q)
const
200 template<
typename ConfigVector,
typename TangentVector>
201 void calc(JointDataDerived & data,
202 const Eigen::MatrixBase<ConfigVector> & q,
203 const Eigen::MatrixBase<TangentVector> & v)
const
206 template<
typename Matrix6Like>
207 void calc_aba(JointDataDerived & data,
const Eigen::MatrixBase<Matrix6Like> & I,
const bool update_I)
const
208 { ::pinocchio::calc_aba(*
this,data,PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like,I),update_I); }
210 std::string shortname()
const { return ::pinocchio::shortname(*
this); }
211 static std::string classname() {
return "JointModel"; }
213 int nq_impl()
const { return ::pinocchio::nq(*
this); }
214 int nv_impl()
const { return ::pinocchio::nv(*
this); }
216 int idx_q_impl()
const { return ::pinocchio::idx_q(*
this); }
217 int idx_v_impl()
const { return ::pinocchio::idx_v(*
this); }
219 JointIndex id_impl()
const { return ::pinocchio::id(*
this); }
221 void setIndexes(JointIndex
id,
int nq,
int nv)
222 { ::pinocchio::setIndexes(*
this,
id,
nq,
nv); }
225 template<
typename NewScalar>
228 return cast_joint<NewScalar,Scalar,Options,JointCollectionTpl>(*
this);
232 typedef PINOCCHIO_ALIGNED_STD_VECTOR(JointData) JointDataVector;
233 typedef PINOCCHIO_ALIGNED_STD_VECTOR(JointModel) JointModelVector;
237 #endif // ifndef __pinocchio_joint_generic_hpp__