Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	include/pinocchio/multibody/joint/joint-free-flyer.hpp	Lines:	63	64	98.4 %
Date:	2024-04-26 13:14:21	Branches:	25	48	52.1 %


//
// Copyright (c) 2015-2019 CNRS INRIA
// Copyright (c) 2015-2016 Wandercraft, 86 rue de Paris 91400 Orsay, France.
//

#ifndef __pinocchio_joint_free_flyer_hpp__
#define __pinocchio_joint_free_flyer_hpp__

#include "pinocchio/macros.hpp"
#include "pinocchio/spatial/inertia.hpp"
#include "pinocchio/spatial/explog.hpp"
#include "pinocchio/multibody/joint/joint-base.hpp"
#include "pinocchio/multibody/constraint.hpp"
#include "pinocchio/math/fwd.hpp"
#include "pinocchio/math/quaternion.hpp"

namespace pinocchio
{

  template<typename Scalar, int Options> struct ConstraintIdentityTpl;

  template<typename _Scalar, int _Options>
  struct traits< ConstraintIdentityTpl<_Scalar,_Options> >
  {
    typedef _Scalar Scalar;
    enum { Options = _Options };
    typedef Eigen::Matrix<Scalar,6,6,Options> Matrix6;
    enum {
      LINEAR = 0,
      ANGULAR = 3
    };
    typedef MotionTpl<Scalar,Options> JointMotion;
    typedef Eigen::Matrix<Scalar,6,1,Options> JointForce;
    typedef Eigen::Matrix<Scalar,6,6,Options> DenseBase;
    typedef typename Matrix6::IdentityReturnType ConstMatrixReturnType;
    typedef typename Matrix6::IdentityReturnType MatrixReturnType;
  }; // traits ConstraintRevolute


  template<typename _Scalar, int _Options>
  struct ConstraintIdentityTpl
  : ConstraintBase< ConstraintIdentityTpl<_Scalar,_Options> >
  {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    PINOCCHIO_CONSTRAINT_TYPEDEF_TPL(ConstraintIdentityTpl)

    enum { NV = 6 };

    template<typename Vector6Like>
    JointMotion __mult__(const Eigen::MatrixBase<Vector6Like> & vj) const
    {
      EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector6Like,6);
      return JointMotion(vj);
    }

    template<typename S1, int O1>
    typename SE3Tpl<S1,O1>::ActionMatrixType
    se3Action(const SE3Tpl<S1,O1> & m) const
    {
      return m.toActionMatrix();

    }

    template<typename S1, int O1>
    typename SE3Tpl<S1,O1>::ActionMatrixType
    se3ActionInverse(const SE3Tpl<S1,O1> & m) const
    {
      return m.toActionMatrixInverse();
    }

    int nv_impl() const { return NV; }

    struct TransposeConst
    {
      template<typename Derived>
      typename ForceDense<Derived>::ToVectorConstReturnType
      operator*(const ForceDense<Derived> & phi)
      {  return phi.toVector();  }

      /* [CRBA]  MatrixBase operator* (Constraint::Transpose S, ForceSet::Block) */
      template<typename MatrixDerived>
      typename PINOCCHIO_EIGEN_REF_CONST_TYPE(MatrixDerived)
      operator*(const Eigen::MatrixBase<MatrixDerived> & F)
      {
        return F.derived();
      }
    };

    TransposeConst transpose() const { return TransposeConst(); }
    MatrixReturnType matrix_impl() const { return DenseBase::Identity(); }

    template<typename MotionDerived>
    typename MotionDerived::ActionMatrixType
    motionAction(const MotionBase<MotionDerived> & v) const
    { return v.toActionMatrix(); }

    bool isEqual(const ConstraintIdentityTpl &) const { return true; }

  }; // struct ConstraintIdentityTpl

  template<typename Scalar, int Options, typename Vector6Like>
  MotionRef<const Vector6Like>
  operator*(const ConstraintIdentityTpl<Scalar,Options> &,
            const Eigen::MatrixBase<Vector6Like> & v)
  {
    EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector6Like,6);
//    typedef typename ConstraintIdentityTpl<Scalar,Options>::Motion Motion;
    typedef MotionRef<const Vector6Like> Motion;
    return Motion(v.derived());
  }

  /* [CRBA] ForceSet operator* (Inertia Y,Constraint S) */
  template<typename S1, int O1, typename S2, int O2>
  inline typename InertiaTpl<S1,O1>::Matrix6
  operator*(const InertiaTpl<S1,O1> & Y, const ConstraintIdentityTpl<S2,O2> &)
  {
    return Y.matrix();
  }

  /* [ABA] Y*S operator*/
  template<typename Matrix6Like, typename S2, int O2>
  inline typename PINOCCHIO_EIGEN_REF_CONST_TYPE(Matrix6Like)
  operator*(const Eigen::MatrixBase<Matrix6Like> & Y, const ConstraintIdentityTpl<S2,O2> &)
  {
    return Y.derived();
  }

  template<typename S1, int O1>
  struct SE3GroupAction< ConstraintIdentityTpl<S1,O1> >
  { typedef typename SE3Tpl<S1,O1>::ActionMatrixType ReturnType; };

  template<typename S1, int O1, typename MotionDerived>
  struct MotionAlgebraAction< ConstraintIdentityTpl<S1,O1>,MotionDerived >
  { typedef typename SE3Tpl<S1,O1>::ActionMatrixType ReturnType; };

  template<typename Scalar, int Options> struct JointFreeFlyerTpl;

  template<typename _Scalar, int _Options>
  struct traits< JointFreeFlyerTpl<_Scalar,_Options> >
  {
    enum {
      NQ = 7,
      NV = 6
    };
    typedef _Scalar Scalar;
    enum { Options = _Options };
    typedef JointDataFreeFlyerTpl<Scalar,Options> JointDataDerived;
    typedef JointModelFreeFlyerTpl<Scalar,Options> JointModelDerived;
    typedef ConstraintIdentityTpl<Scalar,Options> Constraint_t;
    typedef SE3Tpl<Scalar,Options> Transformation_t;
    typedef MotionTpl<Scalar,Options> Motion_t;
    typedef MotionZeroTpl<Scalar,Options> Bias_t;

    // [ABA]
    typedef Eigen::Matrix<Scalar,6,NV,Options> U_t;
    typedef Eigen::Matrix<Scalar,NV,NV,Options> D_t;
    typedef Eigen::Matrix<Scalar,6,NV,Options> UD_t;

    PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE

    typedef Eigen::Matrix<Scalar,NQ,1,Options> ConfigVector_t;
    typedef Eigen::Matrix<Scalar,NV,1,Options> TangentVector_t;
  };

  template<typename Scalar, int Options>
  struct traits< JointDataFreeFlyerTpl<Scalar,Options> >
  { typedef JointFreeFlyerTpl<Scalar,Options> JointDerived; };

  template<typename Scalar, int Options>
  struct traits< JointModelFreeFlyerTpl<Scalar,Options> >
  { typedef JointFreeFlyerTpl<Scalar,Options> JointDerived; };

  template<typename _Scalar, int _Options>
  struct JointDataFreeFlyerTpl : public JointDataBase< JointDataFreeFlyerTpl<_Scalar,_Options> >
  {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    typedef JointFreeFlyerTpl<_Scalar,_Options> JointDerived;
    PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(JointDerived);
    PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR

    Constraint_t S;
    Transformation_t M;
    Motion_t v;
    Bias_t c;

    // [ABA] specific data
    U_t U;
    D_t Dinv;
    UD_t UDinv;

    JointDataFreeFlyerTpl()
    : M(Transformation_t::Identity())
    , v(Motion_t::Zero())
    , U(U_t::Zero())
    , Dinv(D_t::Zero())
    , UDinv(UD_t::Identity())
    {}

    static std::string classname() { return std::string("JointDataFreeFlyer"); }
    std::string shortname() const { return classname(); }

  }; // struct JointDataFreeFlyerTpl

  PINOCCHIO_JOINT_CAST_TYPE_SPECIALIZATION(JointModelFreeFlyerTpl);
  template<typename _Scalar, int _Options>
  struct JointModelFreeFlyerTpl
  : public JointModelBase< JointModelFreeFlyerTpl<_Scalar,_Options> >
  {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    typedef JointFreeFlyerTpl<_Scalar,_Options> JointDerived;
    PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived);

    typedef JointModelBase<JointModelFreeFlyerTpl> Base;
    using Base::id;
    using Base::idx_q;
    using Base::idx_v;
    using Base::setIndexes;

    JointDataDerived createData() const { return JointDataDerived(); }

    const std::vector<bool> hasConfigurationLimit() const
    {
      return {true, true, true, false, false, false, false};
    }

    const std::vector<bool> hasConfigurationLimitInTangent() const
    {
      return {true, true, true, false, false, false};
    }

    template<typename ConfigVectorLike>
    inline void forwardKinematics(Transformation_t & M, const Eigen::MatrixBase<ConfigVectorLike> & q_joint) const
    {
      EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigVector_t,ConfigVectorLike);
      typedef typename Eigen::Quaternion<typename ConfigVectorLike::Scalar,PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorLike)::Options> Quaternion;
      typedef Eigen::Map<const Quaternion> ConstQuaternionMap;

      ConstQuaternionMap quat(q_joint.template tail<4>().data());
      //assert(math::fabs(quat.coeffs().squaredNorm()-1.) <= sqrt(Eigen::NumTraits<typename V::Scalar>::epsilon())); TODO: check validity of the rhs precision
      assert(math::fabs(static_cast<Scalar>(quat.coeffs().squaredNorm()-1)) <= 1e-4);

      M.rotation(quat.matrix());
      M.translation(q_joint.template head<3>());
    }

    template<typename Vector3Derived, typename QuaternionDerived>
    EIGEN_DONT_INLINE
    void calc(JointDataDerived & data,
              const typename Eigen::MatrixBase<Vector3Derived> & trans,
              const typename Eigen::QuaternionBase<QuaternionDerived> & quat) const
    {
      data.M.translation(trans);
      data.M.rotation(quat.matrix());
    }

    template<typename ConfigVector>
    EIGEN_DONT_INLINE
    void calc(JointDataDerived & data,
              const typename Eigen::PlainObjectBase<ConfigVector> & qs) const
    {
      typedef typename Eigen::Quaternion<typename ConfigVector::Scalar,ConfigVector::Options> Quaternion;
      typedef Eigen::Map<const Quaternion> ConstQuaternionMap;

      typename ConfigVector::template ConstFixedSegmentReturnType<NQ>::Type q = qs.template segment<NQ>(idx_q());
      ConstQuaternionMap quat(q.template tail<4>().data());

      calc(data,q.template head<3>(),quat);
    }

    template<typename ConfigVector>
    EIGEN_DONT_INLINE
    void calc(JointDataDerived & data,
              const typename Eigen::MatrixBase<ConfigVector> & qs) const
    {
      typedef typename Eigen::Quaternion<Scalar,Options> Quaternion;

      typename ConfigVector::template ConstFixedSegmentReturnType<NQ>::Type q = qs.template segment<NQ>(idx_q());
      const Quaternion quat(q.template tail<4>());

      calc(data,q.template head<3>(),quat);
    }

    template<typename ConfigVector, typename TangentVector>
    EIGEN_DONT_INLINE
    void calc(JointDataDerived & data,
              const typename Eigen::MatrixBase<ConfigVector> & qs,
              const typename Eigen::MatrixBase<TangentVector> & vs) const
    {
      calc(data,qs.derived());

      data.v = vs.template segment<NV>(idx_v());
    }

    template<typename Matrix6Like>
    void calc_aba(JointDataDerived & data,
                  const Eigen::MatrixBase<Matrix6Like> & I,
                  const bool update_I) const
    {
      data.U = I;

      // compute inverse
//      data.Dinv.setIdentity();
//      I.llt().solveInPlace(data.Dinv);
      internal::PerformStYSInversion<Scalar>::run(I,data.Dinv);

      if (update_I)
        PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like,I).setZero();
    }

    static std::string classname() { return std::string("JointModelFreeFlyer"); }
    std::string shortname() const { return classname(); }

    /// \returns An expression of *this with the Scalar type casted to NewScalar.
    template<typename NewScalar>
    JointModelFreeFlyerTpl<NewScalar,Options> cast() const
    {
      typedef JointModelFreeFlyerTpl<NewScalar,Options> ReturnType;
      ReturnType res;
      res.setIndexes(id(),idx_q(),idx_v());
      return res;
    }

  }; // struct JointModelFreeFlyerTpl

} // namespace pinocchio

#include <boost/type_traits.hpp>

namespace boost
{
  template<typename Scalar, int Options>
  struct has_nothrow_constructor< ::pinocchio::JointModelFreeFlyerTpl<Scalar,Options> >
  : public integral_constant<bool,true> {};

  template<typename Scalar, int Options>
  struct has_nothrow_copy< ::pinocchio::JointModelFreeFlyerTpl<Scalar,Options> >
  : public integral_constant<bool,true> {};

  template<typename Scalar, int Options>
  struct has_nothrow_constructor< ::pinocchio::JointDataFreeFlyerTpl<Scalar,Options> >
  : public integral_constant<bool,true> {};

  template<typename Scalar, int Options>
  struct has_nothrow_copy< ::pinocchio::JointDataFreeFlyerTpl<Scalar,Options> >
  : public integral_constant<bool,true> {};
}

#endif // ifndef __pinocchio_joint_free_flyer_hpp__


Generated by: GCOVR (Version 4.2)

Line	Branch	Exec	Source
1			//
2			// Copyright (c) 2015-2019 CNRS INRIA
3			// Copyright (c) 2015-2016 Wandercraft, 86 rue de Paris 91400 Orsay, France.
4			//
5
6			#ifndef __pinocchio_joint_free_flyer_hpp__
7			#define __pinocchio_joint_free_flyer_hpp__
8
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"
16
17			namespace pinocchio
18			{
19
20			template<typename Scalar, int Options> struct ConstraintIdentityTpl;
21
22			template<typename _Scalar, int _Options>
23			struct traits< ConstraintIdentityTpl<_Scalar,_Options> >
24			{
25			typedef _Scalar Scalar;
26			enum { Options = _Options };
27			typedef Eigen::Matrix<Scalar,6,6,Options> Matrix6;
28			enum {
29			LINEAR = 0,
30			ANGULAR = 3
31			};
32			typedef MotionTpl<Scalar,Options> JointMotion;
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;
37			}; // traits ConstraintRevolute
38
39
40			template<typename _Scalar, int _Options>
41			struct ConstraintIdentityTpl
42			: ConstraintBase< ConstraintIdentityTpl<_Scalar,_Options> >
43			{
44			EIGEN_MAKE_ALIGNED_OPERATOR_NEW
45			PINOCCHIO_CONSTRAINT_TYPEDEF_TPL(ConstraintIdentityTpl)
46
47			enum { NV = 6 };
48
49			template<typename Vector6Like>
50			JointMotion __mult__(const Eigen::MatrixBase<Vector6Like> & vj) const
51			{
52			EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector6Like,6);
53			return JointMotion(vj);
54			}
55
56			template<typename S1, int O1>
57			typename SE3Tpl<S1,O1>::ActionMatrixType
58		844	se3Action(const SE3Tpl<S1,O1> & m) const
59			{
60		844	return m.toActionMatrix();
61
62			}
63
64			template<typename S1, int O1>
65			typename SE3Tpl<S1,O1>::ActionMatrixType
66		13	se3ActionInverse(const SE3Tpl<S1,O1> & m) const
67			{
68		13	return m.toActionMatrixInverse();
69			}
70
71		31	int nv_impl() const { return NV; }
72
73			struct TransposeConst
74			{
75			template<typename Derived>
76			typename ForceDense<Derived>::ToVectorConstReturnType
77		887	operator*(const ForceDense<Derived> & phi)
78		887	{ return phi.toVector(); }
79
80			/* [CRBA] MatrixBase operator* (Constraint::Transpose S, ForceSet::Block) */
81			template<typename MatrixDerived>
82			typename PINOCCHIO_EIGEN_REF_CONST_TYPE(MatrixDerived)
83		65	operator*(const Eigen::MatrixBase<MatrixDerived> & F)
84			{
85		65	return F.derived();
86			}
87			};
88
89		952	TransposeConst transpose() const { return TransposeConst(); }
90		41	MatrixReturnType matrix_impl() const { return DenseBase::Identity(); }
91
92			template<typename MotionDerived>
93			typename MotionDerived::ActionMatrixType
94		4	motionAction(const MotionBase<MotionDerived> & v) const
95		4	{ return v.toActionMatrix(); }
96
97		27	bool isEqual(const ConstraintIdentityTpl &) const { return true; }
98
99			}; // struct ConstraintIdentityTpl
100
101			template<typename Scalar, int Options, typename Vector6Like>
102			MotionRef<const Vector6Like>
103		2080	operator*(const ConstraintIdentityTpl<Scalar,Options> &,
104			const Eigen::MatrixBase<Vector6Like> & v)
105			{
106			EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector6Like,6);
107			// typedef typename ConstraintIdentityTpl<Scalar,Options>::Motion Motion;
108			typedef MotionRef<const Vector6Like> Motion;
109	✓✗	2080	return Motion(v.derived());
110			}
111
112			/* [CRBA] ForceSet operator* (Inertia Y,Constraint S) */
113			template<typename S1, int O1, typename S2, int O2>
114			inline typename InertiaTpl<S1,O1>::Matrix6
115		43	operator*(const InertiaTpl<S1,O1> & Y, const ConstraintIdentityTpl<S2,O2> &)
116			{
117		43	return Y.matrix();
118			}
119
120			/* [ABA] YS operator/
121			template<typename Matrix6Like, typename S2, int O2>
122			inline typename PINOCCHIO_EIGEN_REF_CONST_TYPE(Matrix6Like)
123		2	operator*(const Eigen::MatrixBase<Matrix6Like> & Y, const ConstraintIdentityTpl<S2,O2> &)
124			{
125		2	return Y.derived();
126			}
127
128			template<typename S1, int O1>
129			struct SE3GroupAction< ConstraintIdentityTpl<S1,O1> >
130			{ typedef typename SE3Tpl<S1,O1>::ActionMatrixType ReturnType; };
131
132			template<typename S1, int O1, typename MotionDerived>
133			struct MotionAlgebraAction< ConstraintIdentityTpl<S1,O1>,MotionDerived >
134			{ typedef typename SE3Tpl<S1,O1>::ActionMatrixType ReturnType; };
135
136			template<typename Scalar, int Options> struct JointFreeFlyerTpl;
137
138			template<typename _Scalar, int _Options>
139			struct traits< JointFreeFlyerTpl<_Scalar,_Options> >
140			{
141			enum {
142			NQ = 7,
143			NV = 6
144			};
145			typedef _Scalar Scalar;
146			enum { Options = _Options };
147			typedef JointDataFreeFlyerTpl<Scalar,Options> JointDataDerived;
148			typedef JointModelFreeFlyerTpl<Scalar,Options> JointModelDerived;
149			typedef ConstraintIdentityTpl<Scalar,Options> Constraint_t;
150			typedef SE3Tpl<Scalar,Options> Transformation_t;
151			typedef MotionTpl<Scalar,Options> Motion_t;
152			typedef MotionZeroTpl<Scalar,Options> Bias_t;
153
154			// [ABA]
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;
158
159			PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE
160
161			typedef Eigen::Matrix<Scalar,NQ,1,Options> ConfigVector_t;
162			typedef Eigen::Matrix<Scalar,NV,1,Options> TangentVector_t;
163			};
164
165			template<typename Scalar, int Options>
166			struct traits< JointDataFreeFlyerTpl<Scalar,Options> >
167			{ typedef JointFreeFlyerTpl<Scalar,Options> JointDerived; };
168
169			template<typename Scalar, int Options>
170			struct traits< JointModelFreeFlyerTpl<Scalar,Options> >
171			{ typedef JointFreeFlyerTpl<Scalar,Options> JointDerived; };
172
173			template<typename _Scalar, int _Options>
174			struct JointDataFreeFlyerTpl : public JointDataBase< JointDataFreeFlyerTpl<_Scalar,_Options> >
175			{
176		4304	EIGEN_MAKE_ALIGNED_OPERATOR_NEW
177			typedef JointFreeFlyerTpl<_Scalar,_Options> JointDerived;
178			PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(JointDerived);
179		50728	PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR
180
181			Constraint_t S;
182			Transformation_t M;
183			Motion_t v;
184			Bias_t c;
185
186			// [ABA] specific data
187			U_t U;
188			D_t Dinv;
189			UD_t UDinv;
190
191		3471	JointDataFreeFlyerTpl()
192			: M(Transformation_t::Identity())
193			, v(Motion_t::Zero())
194			, U(U_t::Zero())
195			, Dinv(D_t::Zero())
196	✓✗✓✗ ✓✗	3471	, UDinv(UD_t::Identity())
197		3471	{}
198
199	✓✗	44	static std::string classname() { return std::string("JointDataFreeFlyer"); }
200		3	std::string shortname() const { return classname(); }
201
202			}; // struct JointDataFreeFlyerTpl
203
204			PINOCCHIO_JOINT_CAST_TYPE_SPECIALIZATION(JointModelFreeFlyerTpl);
205			template<typename _Scalar, int _Options>
206			struct JointModelFreeFlyerTpl
207			: public JointModelBase< JointModelFreeFlyerTpl<_Scalar,_Options> >
208			{
209		4552	EIGEN_MAKE_ALIGNED_OPERATOR_NEW
210			typedef JointFreeFlyerTpl<_Scalar,_Options> JointDerived;
211			PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived);
212
213			typedef JointModelBase<JointModelFreeFlyerTpl> Base;
214			using Base::id;
215			using Base::idx_q;
216			using Base::idx_v;
217			using Base::setIndexes;
218
219		3438	JointDataDerived createData() const { return JointDataDerived(); }
220
221		2	const std::vector<bool> hasConfigurationLimit() const
222			{
223	✓✗	2	return {true, true, true, false, false, false, false};
224			}
225
226		2	const std::vector<bool> hasConfigurationLimitInTangent() const
227			{
228	✓✗	2	return {true, true, true, false, false, false};
229			}
230
231			template<typename ConfigVectorLike>
232			inline void forwardKinematics(Transformation_t & M, const Eigen::MatrixBase<ConfigVectorLike> & q_joint) const
233			{
234			EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigVector_t,ConfigVectorLike);
235			typedef typename Eigen::Quaternion<typename ConfigVectorLike::Scalar,PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorLike)::Options> Quaternion;
236			typedef Eigen::Map<const Quaternion> ConstQuaternionMap;
237
238			ConstQuaternionMap quat(q_joint.template tail<4>().data());
239			//assert(math::fabs(quat.coeffs().squaredNorm()-1.) <= sqrt(Eigen::NumTraits<typename V::Scalar>::epsilon())); TODO: check validity of the rhs precision
240			assert(math::fabs(static_cast<Scalar>(quat.coeffs().squaredNorm()-1)) <= 1e-4);
241
242			M.rotation(quat.matrix());
243			M.translation(q_joint.template head<3>());
244			}
245
246			template<typename Vector3Derived, typename QuaternionDerived>
247			EIGEN_DONT_INLINE
248		22692	void calc(JointDataDerived & data,
249			const typename Eigen::MatrixBase<Vector3Derived> & trans,
250			const typename Eigen::QuaternionBase<QuaternionDerived> & quat) const
251			{
252	✓✗	22692	data.M.translation(trans);
253	✓✗	22692	data.M.rotation(quat.matrix());
254		22692	}
255
256			template<typename ConfigVector>
257			EIGEN_DONT_INLINE
258		22683	void calc(JointDataDerived & data,
259			const typename Eigen::PlainObjectBase<ConfigVector> & qs) const
260			{
261			typedef typename Eigen::Quaternion<typename ConfigVector::Scalar,ConfigVector::Options> Quaternion;
262			typedef Eigen::Map<const Quaternion> ConstQuaternionMap;
263
264	✓✗✓✗	22683	typename ConfigVector::template ConstFixedSegmentReturnType<NQ>::Type q = qs.template segment<NQ>(idx_q());
265	✓✗✓✗	22683	ConstQuaternionMap quat(q.template tail<4>().data());
266
267	✓✗✓✗	22683	calc(data,q.template head<3>(),quat);
268		22683	}
269
270			template<typename ConfigVector>
271			EIGEN_DONT_INLINE
272		1	void calc(JointDataDerived & data,
273			const typename Eigen::MatrixBase<ConfigVector> & qs) const
274			{
275			typedef typename Eigen::Quaternion<Scalar,Options> Quaternion;
276
277	✓✗✓✗	1	typename ConfigVector::template ConstFixedSegmentReturnType<NQ>::Type q = qs.template segment<NQ>(idx_q());
278	✓✗✓✗	1	const Quaternion quat(q.template tail<4>());
279
280	✓✗✓✗	1	calc(data,q.template head<3>(),quat);
281		1	}
282
283			template<typename ConfigVector, typename TangentVector>
284			EIGEN_DONT_INLINE
285		3509	void calc(JointDataDerived & data,
286			const typename Eigen::MatrixBase<ConfigVector> & qs,
287			const typename Eigen::MatrixBase<TangentVector> & vs) const
288			{
289		3509	calc(data,qs.derived());
290
291	✓✗	3509	data.v = vs.template segment<NV>(idx_v());
292		3509	}
293
294			template<typename Matrix6Like>
295		264	void calc_aba(JointDataDerived & data,
296			const Eigen::MatrixBase<Matrix6Like> & I,
297			const bool update_I) const
298			{
299		264	data.U = I;
300
301			// compute inverse
302			// data.Dinv.setIdentity();
303			// I.llt().solveInPlace(data.Dinv);
304		264	internal::PerformStYSInversion<Scalar>::run(I,data.Dinv);
305
306	✓✓	264	if (update_I)
307		2	PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like,I).setZero();
308		264	}
309
310	✓✗	16388	static std::string classname() { return std::string("JointModelFreeFlyer"); }
311		16346	std::string shortname() const { return classname(); }
312
313			/// \returns An expression of *this with the Scalar type casted to NewScalar.
314			template<typename NewScalar>
315		9	JointModelFreeFlyerTpl<NewScalar,Options> cast() const
316			{
317			typedef JointModelFreeFlyerTpl<NewScalar,Options> ReturnType;
318		9	ReturnType res;
319		9	res.setIndexes(id(),idx_q(),idx_v());
320		9	return res;
321			}
322
323			}; // struct JointModelFreeFlyerTpl
324
325			} // namespace pinocchio
326
327			#include <boost/type_traits.hpp>
328
329			namespace boost
330			{
331			template<typename Scalar, int Options>
332			struct has_nothrow_constructor< ::pinocchio::JointModelFreeFlyerTpl<Scalar,Options> >
333			: public integral_constant<bool,true> {};
334
335			template<typename Scalar, int Options>
336			struct has_nothrow_copy< ::pinocchio::JointModelFreeFlyerTpl<Scalar,Options> >
337			: public integral_constant<bool,true> {};
338
339			template<typename Scalar, int Options>
340			struct has_nothrow_constructor< ::pinocchio::JointDataFreeFlyerTpl<Scalar,Options> >
341			: public integral_constant<bool,true> {};
342
343			template<typename Scalar, int Options>
344			struct has_nothrow_copy< ::pinocchio::JointDataFreeFlyerTpl<Scalar,Options> >
345			: public integral_constant<bool,true> {};
346			}
347
348			#endif // ifndef __pinocchio_joint_free_flyer_hpp__