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(); }
101 template<
typename Scalar,
int Options,
typename Vector6Like>
104 const Eigen::MatrixBase<Vector6Like> & v)
106 EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector6Like,6);
109 return Motion(v.derived());
113 template<
typename S1,
int O1,
typename S2,
int O2>
121 template<
typename Matrix6Like,
typename S2,
int O2>
122 inline typename PINOCCHIO_EIGEN_REF_CONST_TYPE(Matrix6Like)
128 template<
typename S1,
int O1>
132 template<
typename S1,
int O1,
typename MotionDerived>
138 template<
typename _Scalar,
int _Options>
145 typedef _Scalar Scalar;
146 enum { Options = _Options };
155 typedef Eigen::Matrix<Scalar,6,NV,Options> U_t;
156 typedef Eigen::Matrix<Scalar,NV,NV,Options> D_t;
157 typedef Eigen::Matrix<Scalar,6,NV,Options> UD_t;
159 PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE
161 typedef Eigen::Matrix<Scalar,NQ,1,Options> ConfigVector_t;
162 typedef Eigen::Matrix<Scalar,NV,1,Options> TangentVector_t;
165 template<
typename Scalar,
int Options>
169 template<
typename Scalar,
int Options>
173 template<
typename _Scalar,
int _Options>
176 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
178 PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(JointDerived);
179 PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR
192 : M(Transformation_t::Identity())
193 , v(Motion_t::Zero())
196 , UDinv(UD_t::Identity())
199 static std::string classname() {
return std::string(
"JointDataFreeFlyer"); }
200 std::string
shortname()
const {
return classname(); }
205 template<
typename _Scalar,
int _Options>
207 :
public JointModelBase< JointModelFreeFlyerTpl<_Scalar,_Options> >
209 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
211 PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived);
217 using Base::setIndexes;
219 JointDataDerived
createData()
const {
return JointDataDerived(); }
221 template<
typename ConfigVectorLike>
222 inline void forwardKinematics(Transformation_t & M,
const Eigen::MatrixBase<ConfigVectorLike> & q_joint)
const 224 EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigVector_t,ConfigVectorLike);
225 typedef typename Eigen::Quaternion<typename ConfigVectorLike::Scalar,PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorLike)::Options> Quaternion;
226 typedef Eigen::Map<const Quaternion> ConstQuaternionMap;
228 ConstQuaternionMap quat(q_joint.template tail<4>().data());
230 assert(math::fabs(quat.coeffs().squaredNorm()-1.) <= 1e-4);
232 M.rotation(quat.matrix());
233 M.translation(q_joint.template head<3>());
236 template<
typename Vector3Derived,
typename QuaternionDerived>
238 void calc(JointDataDerived & data,
239 const typename Eigen::MatrixBase<Vector3Derived> & trans,
240 const typename Eigen::QuaternionBase<QuaternionDerived> & quat)
const 242 data.M.translation(trans);
243 data.M.rotation(quat.matrix());
246 template<
typename ConfigVector>
248 void calc(JointDataDerived & data,
249 const typename Eigen::PlainObjectBase<ConfigVector> & qs)
const 251 typedef typename Eigen::Quaternion<typename ConfigVector::Scalar,ConfigVector::Options> Quaternion;
252 typedef Eigen::Map<const Quaternion> ConstQuaternionMap;
254 typename ConfigVector::template ConstFixedSegmentReturnType<NQ>::Type q = qs.template segment<NQ>(
idx_q());
255 ConstQuaternionMap quat(q.template tail<4>().data());
257 calc(data,q.template head<3>(),quat);
260 template<
typename ConfigVector>
262 void calc(JointDataDerived & data,
263 const typename Eigen::MatrixBase<ConfigVector> & qs)
const 265 typedef typename Eigen::Quaternion<Scalar,Options> Quaternion;
267 typename ConfigVector::template ConstFixedSegmentReturnType<NQ>::Type q = qs.template segment<NQ>(
idx_q());
268 const Quaternion quat(q.template tail<4>());
270 calc(data,q.template head<3>(),quat);
273 template<
typename ConfigVector,
typename TangentVector>
275 void calc(JointDataDerived & data,
276 const typename Eigen::MatrixBase<ConfigVector> & qs,
277 const typename Eigen::MatrixBase<TangentVector> & vs)
const 279 calc(data,qs.derived());
281 data.v = vs.template segment<NV>(
idx_v());
284 template<
typename Matrix6Like>
285 void calc_aba(JointDataDerived & data,
286 const Eigen::MatrixBase<Matrix6Like> & I,
287 const bool update_I)
const 294 internal::PerformStYSInversion<Scalar>::run(I,data.Dinv);
297 PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like,I).setZero();
300 static std::string classname() {
return std::string(
"JointModelFreeFlyer"); }
301 std::string
shortname()
const {
return classname(); }
304 template<
typename NewScalar>
317 #include <boost/type_traits.hpp> 321 template<
typename Scalar,
int Options>
323 :
public integral_constant<bool,true> {};
325 template<
typename Scalar,
int Options>
327 :
public integral_constant<bool,true> {};
329 template<
typename Scalar,
int Options>
331 :
public integral_constant<bool,true> {};
333 template<
typename Scalar,
int Options>
335 :
public integral_constant<bool,true> {};
338 #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)