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;
50 typedef U_t UTypeConstRef;
52 typedef D_t DTypeConstRef;
53 typedef UD_t UDTypeConstRef;
55 typedef Eigen::Matrix<Scalar,Eigen::Dynamic,1,Options> ConfigVector_t;
56 typedef Eigen::Matrix<Scalar,Eigen::Dynamic,1,Options> TangentVector_t;
59 template<
typename Scalar,
int Options,
template<
typename S,
int O>
class JointCollectionTpl>
63 template<
typename Scalar,
int Options,
template<
typename S,
int O>
class JointCollectionTpl>
67 template<
typename _Scalar,
int _Options,
template<
typename S,
int O>
class JointCollectionTpl>
69 :
public JointDataBase< JointDataTpl<_Scalar,_Options,JointCollectionTpl> >
70 , JointCollectionTpl<_Scalar,_Options>::JointDataVariant
72 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
77 PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(JointDerived);
79 typedef JointCollectionTpl<_Scalar,_Options> JointCollection;
80 typedef typename JointCollection::JointDataVariant JointDataVariant;
82 JointDataVariant & toVariant() {
return *
static_cast<JointDataVariant*
>(
this); }
83 const JointDataVariant & toVariant()
const {
return *
static_cast<const JointDataVariant*
>(
this); }
87 Motion_t v()
const {
return motion(*
this); }
88 Bias_t c()
const {
return bias(*
this); }
91 U_t U()
const {
return u_inertia(*
this); }
98 : JointDataVariant(jdata_variant)
101 template<
typename Jo
intDataDerived>
103 : JointCollection::JointDataVariant((JointDataVariant)jdata.derived())
105 BOOST_MPL_ASSERT((boost::mpl::contains<typename JointDataVariant::types,JointDataDerived>));
110 Transformation_t M_accessor()
const {
return M(); }
111 Motion_t v_accessor()
const {
return v(); }
112 Bias_t c_accessor()
const {
return c(); }
113 U_t U_accessor()
const {
return U(); }
114 D_t Dinv_accessor()
const {
return Dinv(); }
115 UD_t UDinv_accessor()
const {
return UDinv(); }
117 static std::string classname() {
return "JointData"; }
118 std::string
shortname()
const { return ::pinocchio::shortname(*
this); }
122 template<
typename NewScalar,
typename Scalar,
int Options,
template<
typename S,
int O>
class JointCollectionTpl>
128 template<
typename _Scalar,
int _Options,
template<
typename S,
int O>
class JointCollectionTpl>
130 :
JointModelBase< JointModelTpl<_Scalar,_Options,JointCollectionTpl> >
131 , JointCollectionTpl<_Scalar,_Options>::JointModelVariant
133 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
137 PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived);
138 PINOCCHIO_JOINT_USE_INDEXES;
140 typedef JointCollectionTpl<Scalar,Options> JointCollection;
141 typedef typename JointCollection::JointDataVariant JointDataVariant;
142 typedef typename JointCollection::JointModelVariant JointModelVariant;
145 using Base::setIndexes;
146 using Base::operator==;
147 using Base::operator!=;
152 : JointCollection::JointModelVariant(jmodel_variant)
155 template<
typename Jo
intModelDerived>
157 : JointModelVariant((JointModelVariant)jmodel.derived())
159 BOOST_MPL_ASSERT((boost::mpl::contains<typename JointModelVariant::types,JointModelDerived>));
162 JointModelVariant & toVariant()
163 {
return *
static_cast<JointModelVariant*
>(
this); }
165 const JointModelVariant & toVariant()
const 166 {
return *
static_cast<const JointModelVariant*
>(
this); }
169 { return ::pinocchio::createData<Scalar,Options,JointCollectionTpl>(*this); }
172 bool isEqual(
const JointModelTpl & other)
const 174 return Base::isEqual(other)
175 && toVariant() == other.toVariant();
178 template<
typename ConfigVector>
179 void calc(JointDataDerived & data,
180 const Eigen::MatrixBase<ConfigVector> & q)
const 183 template<
typename ConfigVector,
typename TangentVector>
184 void calc(JointDataDerived & data,
185 const Eigen::MatrixBase<ConfigVector> & q,
186 const Eigen::MatrixBase<TangentVector> & v)
const 189 template<
typename Matrix6Like>
190 void calc_aba(JointDataDerived & data,
const Eigen::MatrixBase<Matrix6Like> & I,
const bool update_I)
const 191 { ::pinocchio::calc_aba(*
this,data,PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like,I),update_I); }
193 std::string
shortname()
const { return ::pinocchio::shortname(*
this); }
194 static std::string classname() {
return "JointModel"; }
196 int nq_impl()
const { return ::pinocchio::nq(*
this); }
197 int nv_impl()
const { return ::pinocchio::nv(*
this); }
199 int idx_q_impl()
const { return ::pinocchio::idx_q(*
this); }
200 int idx_v_impl()
const { return ::pinocchio::idx_v(*
this); }
202 JointIndex id_impl()
const { return ::pinocchio::id(*
this); }
205 { ::pinocchio::setIndexes(*
this,
id, nq, nv); }
208 template<
typename NewScalar>
211 return cast_joint<NewScalar,Scalar,Options,JointCollectionTpl>(*this);
220 #endif // ifndef __pinocchio_joint_generic_hpp__
int nv(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointNvVisitor to get the dimension of the joint tangent space...
MotionTpl< Scalar, Options > motion(const JointDataTpl< Scalar, Options, JointCollectionTpl > &jdata)
Visit a JointDataTpl through JointMotionVisitor to get the joint internal motion as a dense motion...
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.
SE3Tpl< Scalar, Options > joint_transform(const JointDataTpl< Scalar, Options, JointCollectionTpl > &jdata)
Visit a JointDataTpl through JointTransformVisitor to get the joint internal transform (transform bet...
Eigen::Matrix< Scalar, 6, Eigen::Dynamic, Options > udinv_inertia(const JointDataTpl< Scalar, Options, JointCollectionTpl > &jdata)
Visit a JointDataTpl through JointUDInvInertiaVisitor to get U*D^{-1} matrix of the inertia matrix de...
JointModelTpl< NewScalar, Options, JointCollectionTpl > cast() const
std::string shortname(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointShortnameVisitor to get the shortname of the derived joint model...
Eigen::Matrix< Scalar, 6, Eigen::Dynamic, Options > u_inertia(const JointDataTpl< Scalar, Options, JointCollectionTpl > &jdata)
Visit a JointDataTpl through JointUInertiaVisitor to get the U matrix of the inertia matrix decomposi...
Main pinocchio namespace.
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...
MotionTpl< Scalar, Options > bias(const JointDataTpl< Scalar, Options, JointCollectionTpl > &jdata)
Visit a JointDataTpl through JointBiasVisitor to get the joint bias as a dense motion.
void calc_first_order(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel, JointDataTpl< Scalar, Options, JointCollectionTpl > &jdata, const Eigen::MatrixBase< ConfigVectorType > &q, const Eigen::MatrixBase< TangentVectorType > &v)
Visit a JointModelTpl and the corresponding JointDataTpl through JointCalcFirstOrderVisitor to comput...
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...
Constraint_t S_accessor() const
Define all the standard accessors.
Eigen::Matrix< Scalar, Eigen::Dynamic, Eigen::Dynamic, Options > dinv_inertia(const JointDataTpl< Scalar, Options, JointCollectionTpl > &jdata)
Visit a JointDataTpl through JointDInvInertiaVisitor to get the D^{-1} matrix of the inertia matrix d...
ConstraintTpl< Eigen::Dynamic, Scalar, Options > constraint_xd(const JointDataTpl< Scalar, Options, JointCollectionTpl > &jdata)
Visit a JointDataVariant through JointConstraintVisitor to get the joint constraint as a dense constr...
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...
void calc_zero_order(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel, JointDataTpl< Scalar, Options, JointCollectionTpl > &jdata, const Eigen::MatrixBase< ConfigVectorType > &q)
Visit a JointModelTpl and the corresponding JointDataTpl through JointCalcZeroOrderVisitor to compute...