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...
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.
JointModelFreeFlyerTpl< NewScalar, Options > cast() const
ToVectorConstReturnType toVector() const
Return the force as an Eigen vector.
ActionMatrixType toActionMatrixInverse() const
The action matrix of .
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...
ActionMatrixType toActionMatrix() const
The action matrix of .
Main pinocchio namespace.
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)
.