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 template<
typename Jo
intDataDerived>
132 bool isEqual(
const JointDataBase<JointDataDerived> & other)
const
134 return ::pinocchio::isEqual(*
this,other.derived());
137 bool isEqual(
const JointDataTpl & other)
const
139 return Base::isEqual(other) && toVariant() == other.toVariant();
142 bool operator==(
const JointDataTpl & other)
const
144 return isEqual(other);
147 bool operator!=(
const JointDataTpl & other)
const
149 return !(*
this == other);
154 template<
typename NewScalar,
typename Scalar,
int Options,
template<
typename S,
int O>
class JointCollectionTpl>
160 template<
typename _Scalar,
int _Options,
template<
typename S,
int O>
class JointCollectionTpl>
162 :
JointModelBase< JointModelTpl<_Scalar,_Options,JointCollectionTpl> >
163 , JointCollectionTpl<_Scalar,_Options>::JointModelVariant
165 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
169 PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived);
172 typedef JointCollectionTpl<Scalar,Options> JointCollection;
173 typedef typename JointCollection::JointDataVariant JointDataVariant;
174 typedef typename JointCollection::JointModelVariant JointModelVariant;
177 using Base::setIndexes;
178 using Base::operator==;
179 using Base::operator!=;
183 JointModelTpl(
const JointModelVariant & jmodel_variant)
184 : JointCollection::JointModelVariant(jmodel_variant)
188 template<
typename Jo
intModelDerived>
189 JointModelTpl(
const JointModelBase<JointModelDerived> & jmodel)
190 : JointModelVariant((JointModelVariant)jmodel.derived())
192 BOOST_MPL_ASSERT((boost::mpl::contains<typename JointModelVariant::types,JointModelDerived>));
195 JointModelVariant & toVariant()
196 {
return *
static_cast<JointModelVariant*
>(
this); }
198 const JointModelVariant & toVariant()
const
199 {
return *
static_cast<const JointModelVariant*
>(
this); }
201 JointDataDerived createData()
const
202 { return ::pinocchio::createData<Scalar,Options,JointCollectionTpl>(*
this); }
204 template<
typename Jo
intModelDerived>
205 bool isEqual(
const JointModelBase<JointModelDerived> & other)
const
207 return ::pinocchio::isEqual(*
this,other.derived());;
210 template<
typename Jo
intModelDerived>
211 bool hasSameIndexes(
const JointModelBase<JointModelDerived> & other)
const
213 return ::pinocchio::hasSameIndexes(*
this,other.derived());
216 bool isEqual(
const JointModelTpl & other)
const
218 return Base::isEqual(other) && toVariant() == other.toVariant();
221 bool operator==(
const JointModelTpl & other)
const
223 return isEqual(other);
226 bool operator!=(
const JointModelTpl & other)
const
228 return !(*
this == other);
231 template<
typename ConfigVector>
232 void calc(JointDataDerived & data,
233 const Eigen::MatrixBase<ConfigVector> & q)
const
236 template<
typename ConfigVector,
typename TangentVector>
237 void calc(JointDataDerived & data,
238 const Eigen::MatrixBase<ConfigVector> & q,
239 const Eigen::MatrixBase<TangentVector> & v)
const
242 template<
typename Matrix6Like>
243 void calc_aba(JointDataDerived & data,
const Eigen::MatrixBase<Matrix6Like> & I,
const bool update_I)
const
244 { ::pinocchio::calc_aba(*
this,data,PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like,I),update_I); }
246 std::string shortname()
const { return ::pinocchio::shortname(*
this); }
247 static std::string classname() {
return "JointModel"; }
249 int nq_impl()
const { return ::pinocchio::nq(*
this); }
250 int nv_impl()
const { return ::pinocchio::nv(*
this); }
252 int idx_q_impl()
const { return ::pinocchio::idx_q(*
this); }
253 int idx_v_impl()
const { return ::pinocchio::idx_v(*
this); }
255 JointIndex id_impl()
const { return ::pinocchio::id(*
this); }
257 void setIndexes(JointIndex
id,
int nq,
int nv)
259 ::pinocchio::setIndexes(*
this,
id,
nq,
nv);
263 template<
typename NewScalar>
266 return cast_joint<NewScalar,Scalar,Options,JointCollectionTpl>(*
this);
270 typedef PINOCCHIO_ALIGNED_STD_VECTOR(JointData) JointDataVector;
271 typedef PINOCCHIO_ALIGNED_STD_VECTOR(JointModel) JointModelVector;
274 template<
typename Scalar,
int Options,
template<
typename S,
int O>
class JointCollectionTpl,
typename JointDataDerived>
275 bool operator==(
const JointDataBase<JointDataDerived> & joint_data,
276 const JointDataTpl<Scalar,Options,JointCollectionTpl> & joint_data_generic)
278 return joint_data_generic == joint_data.derived();
282 template<
typename Scalar,
int Options,
template<
typename S,
int O>
class JointCollectionTpl,
typename JointDataDerived>
283 bool operator!=(
const JointDataBase<JointDataDerived> & joint_data,
284 const JointDataTpl<Scalar,Options,JointCollectionTpl> & joint_data_generic)
286 return joint_data_generic != joint_data.derived();
289 template<
typename Scalar,
int Options,
template<
typename S,
int O>
class JointCollectionTpl,
typename JointModelDerived>
290 bool operator==(
const JointModelBase<JointModelDerived> & joint_model,
291 const JointModelTpl<Scalar,Options,JointCollectionTpl> & joint_model_generic)
293 return joint_model_generic == joint_model.derived();
296 template<
typename Scalar,
int Options,
template<
typename S,
int O>
class JointCollectionTpl,
typename JointModelDerived>
297 bool operator!=(
const JointModelBase<JointModelDerived> & joint_model,
298 const JointModelTpl<Scalar,Options,JointCollectionTpl> & joint_model_generic)
300 return joint_model_generic != joint_model.derived();
305 #endif // ifndef __pinocchio_joint_generic_hpp__