6 #ifndef __pinocchio_joint_free_flyer_hpp__ 7 #define __pinocchio_joint_free_flyer_hpp__ 9 #include "pinocchio/macros.hpp" 10 #include "pinocchio/spatial/inertia.hpp" 11 #include "pinocchio/spatial/explog.hpp" 12 #include "pinocchio/multibody/joint/joint-base.hpp" 13 #include "pinocchio/multibody/constraint.hpp" 14 #include "pinocchio/math/fwd.hpp" 15 #include "pinocchio/math/quaternion.hpp" 22 template<
typename _Scalar,
int _Options>
25 typedef _Scalar Scalar;
26 enum { Options = _Options };
27 typedef Eigen::Matrix<Scalar,6,6,Options> Matrix6;
33 typedef Eigen::Matrix<Scalar,6,1,Options> JointForce;
34 typedef Eigen::Matrix<Scalar,6,6,Options> DenseBase;
35 typedef typename Matrix6::IdentityReturnType ConstMatrixReturnType;
36 typedef typename Matrix6::IdentityReturnType MatrixReturnType;
40 template<
typename _Scalar,
int _Options>
44 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
49 template<
typename Vector6Like>
50 JointMotion __mult__(
const Eigen::MatrixBase<Vector6Like> & vj)
const 52 EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector6Like,6);
56 template<
typename S1,
int O1>
64 template<
typename S1,
int O1>
71 int nv_impl()
const {
return NV; }
75 template<
typename Derived>
81 template<
typename MatrixDerived>
82 typename PINOCCHIO_EIGEN_REF_CONST_TYPE(MatrixDerived)
83 operator*(
const Eigen::MatrixBase<MatrixDerived> & F)
90 MatrixReturnType matrix_impl()
const {
return DenseBase::Identity(); }
92 template<
typename MotionDerived>
93 typename MotionDerived::ActionMatrixType
95 {
return v.toActionMatrix(); }
99 template<
typename Scalar,
int Options,
typename Vector6Like>
102 const Eigen::MatrixBase<Vector6Like> & v)
104 EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector6Like,6);
107 return Motion(v.derived());
111 template<
typename S1,
int O1,
typename S2,
int O2>
119 template<
typename Matrix6Like,
typename S2,
int O2>
120 inline typename PINOCCHIO_EIGEN_REF_CONST_TYPE(Matrix6Like)
126 template<
typename S1,
int O1>
130 template<
typename S1,
int O1,
typename MotionDerived>
136 template<
typename _Scalar,
int _Options>
143 typedef _Scalar Scalar;
144 enum { Options = _Options };
153 typedef Eigen::Matrix<Scalar,6,NV,Options> U_t;
154 typedef Eigen::Matrix<Scalar,NV,NV,Options> D_t;
155 typedef Eigen::Matrix<Scalar,6,NV,Options> UD_t;
157 PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE
159 typedef Eigen::Matrix<Scalar,NQ,1,Options> ConfigVector_t;
160 typedef Eigen::Matrix<Scalar,NV,1,Options> TangentVector_t;
163 template<
typename Scalar,
int Options>
167 template<
typename Scalar,
int Options>
171 template<
typename _Scalar,
int _Options>
174 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
176 PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(JointDerived);
177 PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR
191 static std::string classname() {
return std::string(
"JointDataFreeFlyer"); }
192 std::string
shortname()
const {
return classname(); }
197 template<
typename _Scalar,
int _Options>
199 :
public JointModelBase< JointModelFreeFlyerTpl<_Scalar,_Options> >
201 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
203 PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived);
209 using Base::setIndexes;
211 JointDataDerived
createData()
const {
return JointDataDerived(); }
213 template<
typename ConfigVectorLike>
214 inline void forwardKinematics(Transformation_t & M,
const Eigen::MatrixBase<ConfigVectorLike> & q_joint)
const 216 EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigVector_t,ConfigVectorLike);
217 typedef typename Eigen::Quaternion<typename ConfigVectorLike::Scalar,PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorLike)::Options> Quaternion;
218 typedef Eigen::Map<const Quaternion> ConstQuaternionMap;
220 ConstQuaternionMap quat(q_joint.template tail<4>().data());
222 assert(math::fabs(quat.coeffs().squaredNorm()-1.) <= 1e-4);
224 M.rotation(quat.matrix());
225 M.translation(q_joint.template head<3>());
228 template<
typename Vector3Derived,
typename QuaternionDerived>
230 void calc(JointDataDerived & data,
231 const typename Eigen::MatrixBase<Vector3Derived> & trans,
232 const typename Eigen::QuaternionBase<QuaternionDerived> & quat)
const 234 data.M.translation(trans);
235 data.M.rotation(quat.matrix());
238 template<
typename ConfigVector>
240 void calc(JointDataDerived & data,
241 const typename Eigen::PlainObjectBase<ConfigVector> & qs)
const 243 typedef typename Eigen::Quaternion<typename ConfigVector::Scalar,ConfigVector::Options> Quaternion;
244 typedef Eigen::Map<const Quaternion> ConstQuaternionMap;
246 typename ConfigVector::template ConstFixedSegmentReturnType<NQ>::Type q = qs.template segment<NQ>(
idx_q());
247 ConstQuaternionMap quat(q.template tail<4>().data());
249 calc(data,q.template head<3>(),quat);
252 template<
typename ConfigVector>
254 void calc(JointDataDerived & data,
255 const typename Eigen::MatrixBase<ConfigVector> & qs)
const 257 typedef typename Eigen::Quaternion<Scalar,Options> Quaternion;
259 typename ConfigVector::template ConstFixedSegmentReturnType<NQ>::Type q = qs.template segment<NQ>(
idx_q());
260 const Quaternion quat(q.template tail<4>());
262 calc(data,q.template head<3>(),quat);
265 template<
typename ConfigVector,
typename TangentVector>
267 void calc(JointDataDerived & data,
268 const typename Eigen::MatrixBase<ConfigVector> & qs,
269 const typename Eigen::MatrixBase<TangentVector> & vs)
const 271 calc(data,qs.derived());
273 data.v = vs.template segment<NV>(
idx_v());
276 template<
typename Matrix6Like>
277 void calc_aba(JointDataDerived & data,
278 const Eigen::MatrixBase<Matrix6Like> & I,
279 const bool update_I)
const 286 internal::PerformStYSInversion<Scalar>::run(I,data.Dinv);
289 PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like,I).setZero();
292 static std::string classname() {
return std::string(
"JointModelFreeFlyer"); }
293 std::string
shortname()
const {
return classname(); }
296 template<
typename NewScalar>
309 #include <boost/type_traits.hpp> 313 template<
typename Scalar,
int Options>
315 :
public integral_constant<bool,true> {};
317 template<
typename Scalar,
int Options>
319 :
public integral_constant<bool,true> {};
321 template<
typename Scalar,
int Options>
323 :
public integral_constant<bool,true> {};
325 template<
typename Scalar,
int Options>
327 :
public integral_constant<bool,true> {};
330 #endif // ifndef __pinocchio_joint_free_flyer_hpp__
int idx_q(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointIdxQVisitor to get the index in the full model configuration space...
JointModelFreeFlyerTpl< NewScalar, Options > cast() const
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...
Return type of the ation of a Motion onto an object of type D.
JointDataTpl< Scalar, Options, JointCollectionTpl > createData(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through CreateData visitor to create a JointDataTpl.
void forwardKinematics(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q)
Update the joint placements according to the current joint configuration.
std::string shortname(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointShortnameVisitor to get the shortname of the derived joint model...
ToVectorConstReturnType toVector() const
Return the force as an Eigen vector.
Main pinocchio namespace.
ActionMatrixType toActionMatrixInverse() const
The action matrix of .
ActionMatrixType toActionMatrix() const
The action matrix of .
Common traits structure to fully define base classes for CRTP.
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...
MultiplicationOp< InertiaTpl< Scalar, Options >, ConstraintDerived >::ReturnType operator*(const InertiaTpl< Scalar, Options > &Y, const ConstraintBase< ConstraintDerived > &constraint)