Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	include/pinocchio/multibody/joint/joint-spherical.hpp	Lines:	130	141	92.2 %
Date:	2023-08-09 08:43:58	Branches:	109	230	47.4 %


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

#ifndef __pinocchio_joint_spherical_hpp__
#define __pinocchio_joint_spherical_hpp__

#include "pinocchio/macros.hpp"
#include "pinocchio/multibody/joint/joint-base.hpp"
#include "pinocchio/multibody/constraint.hpp"
#include "pinocchio/math/sincos.hpp"
#include "pinocchio/spatial/inertia.hpp"
#include "pinocchio/spatial/skew.hpp"

namespace pinocchio
{

  template<typename Scalar, int Options = 0> struct MotionSphericalTpl;
  typedef MotionSphericalTpl<double> MotionSpherical;

  template<typename Scalar, int Options>
  struct SE3GroupAction< MotionSphericalTpl<Scalar,Options> >
  {
    typedef MotionTpl<Scalar,Options> ReturnType;
  };

  template<typename Scalar, int Options, typename MotionDerived>
  struct MotionAlgebraAction< MotionSphericalTpl<Scalar,Options>, MotionDerived>
  {
    typedef MotionTpl<Scalar,Options> ReturnType;
  };

  template<typename _Scalar, int _Options>
  struct traits< MotionSphericalTpl<_Scalar,_Options> >
  {
    typedef _Scalar Scalar;
    enum { Options = _Options };
    typedef Eigen::Matrix<Scalar,3,1,Options> Vector3;
    typedef Eigen::Matrix<Scalar,6,1,Options> Vector6;
    typedef Eigen::Matrix<Scalar,4,4,Options> Matrix4;
    typedef Eigen::Matrix<Scalar,6,6,Options> Matrix6;
    typedef typename PINOCCHIO_EIGEN_REF_CONST_TYPE(Vector6) ToVectorConstReturnType;
    typedef typename PINOCCHIO_EIGEN_REF_TYPE(Vector6) ToVectorReturnType;
    typedef Vector3 AngularType;
    typedef Vector3 LinearType;
    typedef const Vector3 ConstAngularType;
    typedef const Vector3 ConstLinearType;
    typedef Matrix6 ActionMatrixType;
    typedef Matrix4 HomogeneousMatrixType;
    typedef MotionTpl<Scalar,Options> MotionPlain;
    typedef MotionPlain PlainReturnType;
    enum {
      LINEAR = 0,
      ANGULAR = 3
    };
  }; // traits MotionSphericalTpl

  template<typename _Scalar, int _Options>
  struct MotionSphericalTpl
  : MotionBase< MotionSphericalTpl<_Scalar,_Options> >
  {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    MOTION_TYPEDEF_TPL(MotionSphericalTpl);

    MotionSphericalTpl() {}

    template<typename Vector3Like>
    MotionSphericalTpl(const Eigen::MatrixBase<Vector3Like> & w)
    : m_w(w)
    {}

    Vector3 & operator() () { return m_w; }
    const Vector3 & operator() () const { return m_w; }

    inline PlainReturnType plain() const
    {
      return PlainReturnType(PlainReturnType::Vector3::Zero(), m_w);
    }

    template<typename MotionDerived>
    void addTo(MotionDense<MotionDerived> & other) const
    {
      other.angular() += m_w;
    }

    template<typename Derived>
    void setTo(MotionDense<Derived> & other) const
    {
      other.linear().setZero();
      other.angular() = m_w;
    }

    MotionSphericalTpl __plus__(const MotionSphericalTpl & other) const
    {
      return MotionSphericalTpl(m_w + other.m_w);
    }

    bool isEqual_impl(const MotionSphericalTpl & other) const
    {
      return m_w == other.m_w;
    }

    template<typename MotionDerived>
    bool isEqual_impl(const MotionDense<MotionDerived> & other) const
    {
      return other.angular() == m_w && other.linear().isZero(0);
    }

    template<typename S2, int O2, typename D2>
    void se3Action_impl(const SE3Tpl<S2,O2> & m, MotionDense<D2> & v) const
    {
      // Angular
      v.angular().noalias() = m.rotation() * m_w;

      // Linear
      v.linear().noalias() = m.translation().cross(v.angular());
    }

    template<typename S2, int O2>
    MotionPlain se3Action_impl(const SE3Tpl<S2,O2> & m) const
    {
      MotionPlain res;
      se3Action_impl(m,res);
      return res;
    }

    template<typename S2, int O2, typename D2>
    void se3ActionInverse_impl(const SE3Tpl<S2,O2> & m, MotionDense<D2> & v) const
    {
      // Linear
      // TODO: use v.angular() as temporary variable
      Vector3 v3_tmp;
      v3_tmp.noalias() = m_w.cross(m.translation());
      v.linear().noalias() = m.rotation().transpose() * v3_tmp;

      // Angular
      v.angular().noalias() = m.rotation().transpose() * m_w;
    }

    template<typename S2, int O2>
    MotionPlain se3ActionInverse_impl(const SE3Tpl<S2,O2> & m) const
    {
      MotionPlain res;
      se3ActionInverse_impl(m,res);
      return res;
    }

    template<typename M1, typename M2>
    void motionAction(const MotionDense<M1> & v, MotionDense<M2> & mout) const
    {
      // Linear
      mout.linear().noalias() = v.linear().cross(m_w);

      // Angular
      mout.angular().noalias() = v.angular().cross(m_w);
    }

    template<typename M1>
    MotionPlain motionAction(const MotionDense<M1> & v) const
    {
      MotionPlain res;
      motionAction(v,res);
      return res;
    }

    const Vector3 & angular() const { return m_w; }
    Vector3 & angular() { return m_w; }

  protected:

    Vector3 m_w;
  }; // struct MotionSphericalTpl

  template<typename S1, int O1, typename MotionDerived>
  inline typename MotionDerived::MotionPlain
  operator+(const MotionSphericalTpl<S1,O1> & m1, const MotionDense<MotionDerived> & m2)
  {
    return typename MotionDerived::MotionPlain(m2.linear(),m2.angular() + m1.angular());
  }

  template<typename Scalar, int Options> struct ConstraintSphericalTpl;

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

  template<typename _Scalar, int _Options>
  struct ConstraintSphericalTpl
  : public ConstraintBase< ConstraintSphericalTpl<_Scalar,_Options> >
  {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    PINOCCHIO_CONSTRAINT_TYPEDEF_TPL(ConstraintSphericalTpl)

    ConstraintSphericalTpl() {}

    enum { NV = 3 };

    int nv_impl() const { return NV; }

    template<typename Vector3Like>
    JointMotion __mult__(const Eigen::MatrixBase<Vector3Like> & w) const
    {
      EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3Like,3);
      return JointMotion(w);
    }

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

      /* [CRBA]  MatrixBase operator* (Constraint::Transpose S, ForceSet::Block) */
      template<typename MatrixDerived>
      const typename SizeDepType<3>::RowsReturn<MatrixDerived>::ConstType
      operator*(const Eigen::MatrixBase<MatrixDerived> & F) const
      {
        assert(F.rows()==6);
        return F.derived().template middleRows<3>(Inertia::ANGULAR);
      }
    };

    TransposeConst transpose() const { return TransposeConst(); }

    DenseBase matrix_impl() const
    {
      DenseBase S;
      S.template block <3,3>(LINEAR,0).setZero();
      S.template block <3,3>(ANGULAR,0).setIdentity();
      return S;
    }

    template<typename S1, int O1>
    Eigen::Matrix<S1,6,3,O1> se3Action(const SE3Tpl<S1,O1> & m) const
    {
      Eigen::Matrix<S1,6,3,O1> X_subspace;
      cross(m.translation(),m.rotation(),X_subspace.template middleRows<3>(LINEAR));
      X_subspace.template middleRows<3>(ANGULAR) = m.rotation();

      return X_subspace;
    }

    template<typename S1, int O1>
    Eigen::Matrix<S1,6,3,O1> se3ActionInverse(const SE3Tpl<S1,O1> & m) const
    {
      Eigen::Matrix<S1,6,3,O1> X_subspace;
      AxisX::cross(m.translation(),X_subspace.template middleRows<3>(ANGULAR).col(0));
      AxisY::cross(m.translation(),X_subspace.template middleRows<3>(ANGULAR).col(1));
      AxisZ::cross(m.translation(),X_subspace.template middleRows<3>(ANGULAR).col(2));

      X_subspace.template middleRows<3>(LINEAR).noalias()
      = m.rotation().transpose() * X_subspace.template middleRows<3>(ANGULAR);
      X_subspace.template middleRows<3>(ANGULAR) = m.rotation().transpose();

      return X_subspace;
    }

    template<typename MotionDerived>
    DenseBase motionAction(const MotionDense<MotionDerived> & m) const
    {
      const typename MotionDerived::ConstLinearType v = m.linear();
      const typename MotionDerived::ConstAngularType w = m.angular();

      DenseBase res;
      skew(v,res.template middleRows<3>(LINEAR));
      skew(w,res.template middleRows<3>(ANGULAR));

      return res;
    }

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

  }; // struct ConstraintSphericalTpl

  template<typename MotionDerived, typename S2, int O2>
  inline typename MotionDerived::MotionPlain
  operator^(const MotionDense<MotionDerived> & m1,
            const MotionSphericalTpl<S2,O2> & m2)
  {
    return m2.motionAction(m1);
  }

  /* [CRBA] ForceSet operator* (Inertia Y,Constraint S) */
  template<typename S1, int O1, typename S2, int O2>
  inline Eigen::Matrix<S2,6,3,O2>
  operator*(const InertiaTpl<S1,O1> & Y,
            const ConstraintSphericalTpl<S2,O2> &)
  {
    typedef InertiaTpl<S1,O1> Inertia;
    typedef typename Inertia::Symmetric3 Symmetric3;
    Eigen::Matrix<S2,6,3,O2> M;
    //    M.block <3,3> (Inertia::LINEAR, 0) = - Y.mass () * skew(Y.lever ());
    M.template block<3,3>(Inertia::LINEAR,0) = alphaSkew(-Y.mass(), Y.lever());
    M.template block<3,3>(Inertia::ANGULAR,0) = (Y.inertia() - typename Symmetric3::AlphaSkewSquare(Y.mass(), Y.lever())).matrix();
    return M;
  }

  /* [ABA] Y*S operator*/
  template<typename M6Like, typename S2, int O2>
  inline typename SizeDepType<3>::ColsReturn<M6Like>::ConstType
  operator*(const Eigen::MatrixBase<M6Like> & Y,
            const ConstraintSphericalTpl<S2,O2> &)
  {
    typedef ConstraintSphericalTpl<S2,O2> Constraint;
    return Y.derived().template middleCols<3>(Constraint::ANGULAR);
  }

  template<typename S1, int O1>
  struct SE3GroupAction< ConstraintSphericalTpl<S1,O1> >
  { typedef Eigen::Matrix<S1,6,3,O1> ReturnType; };

  template<typename S1, int O1, typename MotionDerived>
  struct MotionAlgebraAction< ConstraintSphericalTpl<S1,O1>,MotionDerived >
  { typedef Eigen::Matrix<S1,6,3,O1> ReturnType; };

  template<typename Scalar, int Options> struct JointSphericalTpl;

  template<typename _Scalar, int _Options>
  struct traits< JointSphericalTpl<_Scalar,_Options> >
  {
    enum {
      NQ = 4,
      NV = 3
    };
    typedef _Scalar Scalar;
    enum { Options = _Options };
    typedef JointDataSphericalTpl<Scalar,Options> JointDataDerived;
    typedef JointModelSphericalTpl<Scalar,Options> JointModelDerived;
    typedef ConstraintSphericalTpl<Scalar,Options> Constraint_t;
    typedef SE3Tpl<Scalar,Options> Transformation_t;
    typedef MotionSphericalTpl<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< JointDataSphericalTpl<Scalar,Options> >
  { typedef JointSphericalTpl<Scalar,Options> JointDerived; };

  template<typename Scalar, int Options>
  struct traits< JointModelSphericalTpl<Scalar,Options> >
  { typedef JointSphericalTpl<Scalar,Options> JointDerived; };

  template<typename _Scalar, int _Options>
  struct JointDataSphericalTpl : public JointDataBase< JointDataSphericalTpl<_Scalar,_Options> >
  {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    typedef JointSphericalTpl<_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;

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

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

  }; // struct JointDataSphericalTpl

  PINOCCHIO_JOINT_CAST_TYPE_SPECIALIZATION(JointModelSphericalTpl);
  template<typename _Scalar, int _Options>
  struct JointModelSphericalTpl
  : public JointModelBase< JointModelSphericalTpl<_Scalar,_Options> >
  {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    typedef JointSphericalTpl<_Scalar,_Options> JointDerived;
    PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived);

    typedef JointModelBase<JointModelSphericalTpl> 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 {false, false, false, false};
    }

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

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

      ConstQuaternionMap quat(q_joint.derived().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().setZero();
    }

    template<typename QuaternionDerived>
    void calc(JointDataDerived & data,
              const typename Eigen::QuaternionBase<QuaternionDerived> & quat) const
    {
      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;

      ConstQuaternionMap quat(qs.template segment<NQ>(idx_q()).data());
      calc(data,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;

      const Quaternion quat(qs.template segment<NQ>(idx_q()));
      calc(data,quat);
    }

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

      data.v.angular() = 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.template block<6,3>(0,Inertia::ANGULAR);

      // compute inverse
//      data.Dinv.setIdentity();
//      data.U.template middleRows<3>(Inertia::ANGULAR).llt().solveInPlace(data.Dinv);
      internal::PerformStYSInversion<Scalar>::run(data.U.template middleRows<3>(Inertia::ANGULAR),data.Dinv);

      data.UDinv.template middleRows<3>(Inertia::ANGULAR).setIdentity(); // can be put in data constructor
      data.UDinv.template middleRows<3>(Inertia::LINEAR).noalias() = data.U.template block<3,3>(Inertia::LINEAR, 0) * data.Dinv;

      if (update_I)
      {
        Matrix6Like & I_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like,I);
        I_.template block<3,3>(Inertia::LINEAR,Inertia::LINEAR)
        -= data.UDinv.template middleRows<3>(Inertia::LINEAR) * I_.template block<3,3> (Inertia::ANGULAR, Inertia::LINEAR);
        I_.template block<6,3>(0,Inertia::ANGULAR).setZero();
        I_.template block<3,3>(Inertia::ANGULAR,Inertia::LINEAR).setZero();
      }
    }

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

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

  }; // struct JointModelSphericalTpl

} // namespace pinocchio

#include <boost/type_traits.hpp>

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

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

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

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

#endif // ifndef __pinocchio_joint_spherical_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_spherical_hpp__
7			#define __pinocchio_joint_spherical_hpp__
8
9			#include "pinocchio/macros.hpp"
10			#include "pinocchio/multibody/joint/joint-base.hpp"
11			#include "pinocchio/multibody/constraint.hpp"
12			#include "pinocchio/math/sincos.hpp"
13			#include "pinocchio/spatial/inertia.hpp"
14			#include "pinocchio/spatial/skew.hpp"
15
16			namespace pinocchio
17			{
18
19			template<typename Scalar, int Options = 0> struct MotionSphericalTpl;
20			typedef MotionSphericalTpl<double> MotionSpherical;
21
22			template<typename Scalar, int Options>
23			struct SE3GroupAction< MotionSphericalTpl<Scalar,Options> >
24			{
25			typedef MotionTpl<Scalar,Options> ReturnType;
26			};
27
28			template<typename Scalar, int Options, typename MotionDerived>
29			struct MotionAlgebraAction< MotionSphericalTpl<Scalar,Options>, MotionDerived>
30			{
31			typedef MotionTpl<Scalar,Options> ReturnType;
32			};
33
34			template<typename _Scalar, int _Options>
35			struct traits< MotionSphericalTpl<_Scalar,_Options> >
36			{
37			typedef _Scalar Scalar;
38			enum { Options = _Options };
39			typedef Eigen::Matrix<Scalar,3,1,Options> Vector3;
40			typedef Eigen::Matrix<Scalar,6,1,Options> Vector6;
41			typedef Eigen::Matrix<Scalar,4,4,Options> Matrix4;
42			typedef Eigen::Matrix<Scalar,6,6,Options> Matrix6;
43			typedef typename PINOCCHIO_EIGEN_REF_CONST_TYPE(Vector6) ToVectorConstReturnType;
44			typedef typename PINOCCHIO_EIGEN_REF_TYPE(Vector6) ToVectorReturnType;
45			typedef Vector3 AngularType;
46			typedef Vector3 LinearType;
47			typedef const Vector3 ConstAngularType;
48			typedef const Vector3 ConstLinearType;
49			typedef Matrix6 ActionMatrixType;
50			typedef Matrix4 HomogeneousMatrixType;
51			typedef MotionTpl<Scalar,Options> MotionPlain;
52			typedef MotionPlain PlainReturnType;
53			enum {
54			LINEAR = 0,
55			ANGULAR = 3
56			};
57			}; // traits MotionSphericalTpl
58
59			template<typename _Scalar, int _Options>
60			struct MotionSphericalTpl
61			: MotionBase< MotionSphericalTpl<_Scalar,_Options> >
62			{
63			EIGEN_MAKE_ALIGNED_OPERATOR_NEW
64
65			MOTION_TYPEDEF_TPL(MotionSphericalTpl);
66
67		21	MotionSphericalTpl() {}
68
69			template<typename Vector3Like>
70		9674	MotionSphericalTpl(const Eigen::MatrixBase<Vector3Like> & w)
71		9674	: m_w(w)
72		9674	{}
73
74		4140	Vector3 & operator() () { return m_w; }
75			const Vector3 & operator() () const { return m_w; }
76
77		2	inline PlainReturnType plain() const
78			{
79	✓✗	2	return PlainReturnType(PlainReturnType::Vector3::Zero(), m_w);
80			}
81
82			template<typename MotionDerived>
83		8	void addTo(MotionDense<MotionDerived> & other) const
84			{
85	✓✗	8	other.angular() += m_w;
86		8	}
87
88			template<typename Derived>
89		2200	void setTo(MotionDense<Derived> & other) const
90			{
91	✓✗	2200	other.linear().setZero();
92	✓✗	2200	other.angular() = m_w;
93		2200	}
94
95			MotionSphericalTpl __plus__(const MotionSphericalTpl & other) const
96			{
97			return MotionSphericalTpl(m_w + other.m_w);
98			}
99
100		51	bool isEqual_impl(const MotionSphericalTpl & other) const
101			{
102		51	return m_w == other.m_w;
103			}
104
105			template<typename MotionDerived>
106		1	bool isEqual_impl(const MotionDense<MotionDerived> & other) const
107			{
108	✓✗✓✗ ✓✗✓✗ ✓✗✓✗	1	return other.angular() == m_w && other.linear().isZero(0);
109			}
110
111			template<typename S2, int O2, typename D2>
112		2	void se3Action_impl(const SE3Tpl<S2,O2> & m, MotionDense<D2> & v) const
113			{
114			// Angular
115	✓✗✓✗ ✓✗	2	v.angular().noalias() = m.rotation() * m_w;
116
117			// Linear
118	✓✗✓✗ ✓✗✓✗	2	v.linear().noalias() = m.translation().cross(v.angular());
119		2	}
120
121			template<typename S2, int O2>
122		2	MotionPlain se3Action_impl(const SE3Tpl<S2,O2> & m) const
123			{
124		2	MotionPlain res;
125		2	se3Action_impl(m,res);
126		2	return res;
127			}
128
129			template<typename S2, int O2, typename D2>
130		2	void se3ActionInverse_impl(const SE3Tpl<S2,O2> & m, MotionDense<D2> & v) const
131			{
132			// Linear
133			// TODO: use v.angular() as temporary variable
134	✓✗	2	Vector3 v3_tmp;
135	✓✗✓✗ ✓✗✓✗	2	v3_tmp.noalias() = m_w.cross(m.translation());
136	✓✗✓✗ ✓✗✓✗ ✓✗✓✗	2	v.linear().noalias() = m.rotation().transpose() * v3_tmp;
137
138			// Angular
139	✓✗✓✗ ✓✗✓✗ ✓✗✓✗	2	v.angular().noalias() = m.rotation().transpose() * m_w;
140		2	}
141
142			template<typename S2, int O2>
143		2	MotionPlain se3ActionInverse_impl(const SE3Tpl<S2,O2> & m) const
144			{
145		2	MotionPlain res;
146		2	se3ActionInverse_impl(m,res);
147		2	return res;
148			}
149
150			template<typename M1, typename M2>
151		124	void motionAction(const MotionDense<M1> & v, MotionDense<M2> & mout) const
152			{
153			// Linear
154	✓✗✓✗ ✓✗✓✗	124	mout.linear().noalias() = v.linear().cross(m_w);
155
156			// Angular
157	✓✗✓✗ ✓✗✓✗	124	mout.angular().noalias() = v.angular().cross(m_w);
158		124	}
159
160			template<typename M1>
161		124	MotionPlain motionAction(const MotionDense<M1> & v) const
162			{
163		124	MotionPlain res;
164		124	motionAction(v,res);
165		124	return res;
166			}
167
168		116	const Vector3 & angular() const { return m_w; }
169		1244	Vector3 & angular() { return m_w; }
170
171			protected:
172
173			Vector3 m_w;
174			}; // struct MotionSphericalTpl
175
176			template<typename S1, int O1, typename MotionDerived>
177			inline typename MotionDerived::MotionPlain
178		116	operator+(const MotionSphericalTpl<S1,O1> & m1, const MotionDense<MotionDerived> & m2)
179			{
180	✓✗✓✗ ✓✗	116	return typename MotionDerived::MotionPlain(m2.linear(),m2.angular() + m1.angular());
181			}
182
183			template<typename Scalar, int Options> struct ConstraintSphericalTpl;
184
185			template<typename _Scalar, int _Options>
186			struct traits< ConstraintSphericalTpl<_Scalar,_Options> >
187			{
188			typedef _Scalar Scalar;
189			enum { Options = _Options };
190			enum {
191			LINEAR = 0,
192			ANGULAR = 3
193			};
194			typedef MotionSphericalTpl<Scalar,Options> JointMotion;
195			typedef Eigen::Matrix<Scalar,3,1,Options> JointForce;
196			typedef Eigen::Matrix<Scalar,6,3,Options> DenseBase;
197			typedef DenseBase MatrixReturnType;
198			typedef const DenseBase ConstMatrixReturnType;
199			}; // struct traits struct ConstraintSphericalTpl
200
201			template<typename _Scalar, int _Options>
202			struct ConstraintSphericalTpl
203			: public ConstraintBase< ConstraintSphericalTpl<_Scalar,_Options> >
204			{
205			EIGEN_MAKE_ALIGNED_OPERATOR_NEW
206
207			PINOCCHIO_CONSTRAINT_TYPEDEF_TPL(ConstraintSphericalTpl)
208
209		3145	ConstraintSphericalTpl() {}
210
211			enum { NV = 3 };
212
213		22	int nv_impl() const { return NV; }
214
215			template<typename Vector3Like>
216		126	JointMotion __mult__(const Eigen::MatrixBase<Vector3Like> & w) const
217			{
218			EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3Like,3);
219		126	return JointMotion(w);
220			}
221
222			struct TransposeConst
223			{
224			template<typename Derived>
225			typename ForceDense<Derived>::ConstAngularType
226		7	operator* (const ForceDense<Derived> & phi)
227		7	{ return phi.angular(); }
228
229			/* [CRBA] MatrixBase operator* (Constraint::Transpose S, ForceSet::Block) */
230			template<typename MatrixDerived>
231			const typename SizeDepType<3>::RowsReturn<MatrixDerived>::ConstType
232		7	operator*(const Eigen::MatrixBase<MatrixDerived> & F) const
233			{
234	✗✓	7	assert(F.rows()==6);
235		7	return F.derived().template middleRows<3>(Inertia::ANGULAR);
236			}
237			};
238
239		14	TransposeConst transpose() const { return TransposeConst(); }
240
241		21	DenseBase matrix_impl() const
242			{
243		21	DenseBase S;
244	✓✗	21	S.template block <3,3>(LINEAR,0).setZero();
245	✓✗	21	S.template block <3,3>(ANGULAR,0).setIdentity();
246		21	return S;
247			}
248
249			template<typename S1, int O1>
250		54	Eigen::Matrix<S1,6,3,O1> se3Action(const SE3Tpl<S1,O1> & m) const
251			{
252		54	Eigen::Matrix<S1,6,3,O1> X_subspace;
253	✓✗✓✗ ✓✗	54	cross(m.translation(),m.rotation(),X_subspace.template middleRows<3>(LINEAR));
254	✓✗	54	X_subspace.template middleRows<3>(ANGULAR) = m.rotation();
255
256		54	return X_subspace;
257			}
258
259			template<typename S1, int O1>
260		3	Eigen::Matrix<S1,6,3,O1> se3ActionInverse(const SE3Tpl<S1,O1> & m) const
261			{
262		3	Eigen::Matrix<S1,6,3,O1> X_subspace;
263	✓✗✓✗ ✓✗	3	AxisX::cross(m.translation(),X_subspace.template middleRows<3>(ANGULAR).col(0));
264	✓✗✓✗ ✓✗	3	AxisY::cross(m.translation(),X_subspace.template middleRows<3>(ANGULAR).col(1));
265	✓✗✓✗ ✓✗	3	AxisZ::cross(m.translation(),X_subspace.template middleRows<3>(ANGULAR).col(2));
266
267			X_subspace.template middleRows<3>(LINEAR).noalias()
268	✓✗✓✗ ✓✗✓✗ ✓✗✓✗	3	= m.rotation().transpose() * X_subspace.template middleRows<3>(ANGULAR);
269	✓✗✓✗	3	X_subspace.template middleRows<3>(ANGULAR) = m.rotation().transpose();
270
271		3	return X_subspace;
272			}
273
274			template<typename MotionDerived>
275		2	DenseBase motionAction(const MotionDense<MotionDerived> & m) const
276			{
277	✓✗	2	const typename MotionDerived::ConstLinearType v = m.linear();
278	✓✗	2	const typename MotionDerived::ConstAngularType w = m.angular();
279
280	✓✗	2	DenseBase res;
281	✓✗✓✗	2	skew(v,res.template middleRows<3>(LINEAR));
282	✓✗✓✗	2	skew(w,res.template middleRows<3>(ANGULAR));
283
284		4	return res;
285			}
286
287		17	bool isEqual(const ConstraintSphericalTpl &) const { return true; }
288
289			}; // struct ConstraintSphericalTpl
290
291			template<typename MotionDerived, typename S2, int O2>
292			inline typename MotionDerived::MotionPlain
293		122	operator^(const MotionDense<MotionDerived> & m1,
294			const MotionSphericalTpl<S2,O2> & m2)
295			{
296		122	return m2.motionAction(m1);
297			}
298
299			/* [CRBA] ForceSet operator* (Inertia Y,Constraint S) */
300			template<typename S1, int O1, typename S2, int O2>
301			inline Eigen::Matrix<S2,6,3,O2>
302		8	operator*(const InertiaTpl<S1,O1> & Y,
303			const ConstraintSphericalTpl<S2,O2> &)
304			{
305			typedef InertiaTpl<S1,O1> Inertia;
306			typedef typename Inertia::Symmetric3 Symmetric3;
307		8	Eigen::Matrix<S2,6,3,O2> M;
308			// M.block <3,3> (Inertia::LINEAR, 0) = - Y.mass () * skew(Y.lever ());
309	✓✗✓✗	8	M.template block<3,3>(Inertia::LINEAR,0) = alphaSkew(-Y.mass(), Y.lever());
310	✓✗✓✗ ✓✗✓✗	8	M.template block<3,3>(Inertia::ANGULAR,0) = (Y.inertia() - typename Symmetric3::AlphaSkewSquare(Y.mass(), Y.lever())).matrix();
311		8	return M;
312			}
313
314			/* [ABA] YS operator/
315			template<typename M6Like, typename S2, int O2>
316			inline typename SizeDepType<3>::ColsReturn<M6Like>::ConstType
317		2	operator*(const Eigen::MatrixBase<M6Like> & Y,
318			const ConstraintSphericalTpl<S2,O2> &)
319			{
320			typedef ConstraintSphericalTpl<S2,O2> Constraint;
321		2	return Y.derived().template middleCols<3>(Constraint::ANGULAR);
322			}
323
324			template<typename S1, int O1>
325			struct SE3GroupAction< ConstraintSphericalTpl<S1,O1> >
326			{ typedef Eigen::Matrix<S1,6,3,O1> ReturnType; };
327
328			template<typename S1, int O1, typename MotionDerived>
329			struct MotionAlgebraAction< ConstraintSphericalTpl<S1,O1>,MotionDerived >
330			{ typedef Eigen::Matrix<S1,6,3,O1> ReturnType; };
331
332			template<typename Scalar, int Options> struct JointSphericalTpl;
333
334			template<typename _Scalar, int _Options>
335			struct traits< JointSphericalTpl<_Scalar,_Options> >
336			{
337			enum {
338			NQ = 4,
339			NV = 3
340			};
341			typedef _Scalar Scalar;
342			enum { Options = _Options };
343			typedef JointDataSphericalTpl<Scalar,Options> JointDataDerived;
344			typedef JointModelSphericalTpl<Scalar,Options> JointModelDerived;
345			typedef ConstraintSphericalTpl<Scalar,Options> Constraint_t;
346			typedef SE3Tpl<Scalar,Options> Transformation_t;
347			typedef MotionSphericalTpl<Scalar,Options> Motion_t;
348			typedef MotionZeroTpl<Scalar,Options> Bias_t;
349
350			// [ABA]
351			typedef Eigen::Matrix<Scalar,6,NV,Options> U_t;
352			typedef Eigen::Matrix<Scalar,NV,NV,Options> D_t;
353			typedef Eigen::Matrix<Scalar,6,NV,Options> UD_t;
354
355			PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE
356
357			typedef Eigen::Matrix<Scalar,NQ,1,Options> ConfigVector_t;
358			typedef Eigen::Matrix<Scalar,NV,1,Options> TangentVector_t;
359			};
360
361			template<typename Scalar, int Options>
362			struct traits< JointDataSphericalTpl<Scalar,Options> >
363			{ typedef JointSphericalTpl<Scalar,Options> JointDerived; };
364
365			template<typename Scalar, int Options>
366			struct traits< JointModelSphericalTpl<Scalar,Options> >
367			{ typedef JointSphericalTpl<Scalar,Options> JointDerived; };
368
369			template<typename _Scalar, int _Options>
370			struct JointDataSphericalTpl : public JointDataBase< JointDataSphericalTpl<_Scalar,_Options> >
371			{
372		108	EIGEN_MAKE_ALIGNED_OPERATOR_NEW
373
374			typedef JointSphericalTpl<_Scalar,_Options> JointDerived;
375			PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(JointDerived);
376		2088	PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR
377
378			Constraint_t S;
379			Transformation_t M;
380			Motion_t v;
381			Bias_t c;
382
383			// [ABA] specific data
384			U_t U;
385			D_t Dinv;
386			UD_t UDinv;
387
388		3138	JointDataSphericalTpl ()
389			: M(Transformation_t::Identity())
390			, v(Motion_t::Vector3::Zero())
391			, U(U_t::Zero())
392			, Dinv(D_t::Zero())
393	✓✗✓✗ ✓✗✓✗	3138	, UDinv(UD_t::Zero())
394		3138	{}
395
396	✓✗	44	static std::string classname() { return std::string("JointDataSpherical"); }
397		3	std::string shortname() const { return classname(); }
398
399			}; // struct JointDataSphericalTpl
400
401			PINOCCHIO_JOINT_CAST_TYPE_SPECIALIZATION(JointModelSphericalTpl);
402			template<typename _Scalar, int _Options>
403			struct JointModelSphericalTpl
404			: public JointModelBase< JointModelSphericalTpl<_Scalar,_Options> >
405			{
406		204	EIGEN_MAKE_ALIGNED_OPERATOR_NEW
407
408			typedef JointSphericalTpl<_Scalar,_Options> JointDerived;
409			PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived);
410
411			typedef JointModelBase<JointModelSphericalTpl> Base;
412			using Base::id;
413			using Base::idx_q;
414			using Base::idx_v;
415			using Base::setIndexes;
416
417		3112	JointDataDerived createData() const { return JointDataDerived(); }
418
419			const std::vector<bool> hasConfigurationLimit() const
420			{
421			return {false, false, false, false};
422			}
423
424			const std::vector<bool> hasConfigurationLimitInTangent() const
425			{
426			return {false, false, false};
427			}
428
429			template<typename ConfigVectorLike>
430			inline void forwardKinematics(Transformation_t & M, const Eigen::MatrixBase<ConfigVectorLike> & q_joint) const
431			{
432			typedef typename ConfigVectorLike::Scalar Scalar;
433			EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigVector_t,ConfigVectorLike);
434			typedef typename Eigen::Quaternion<Scalar,PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorLike)::Options> Quaternion;
435			typedef Eigen::Map<const Quaternion> ConstQuaternionMap;
436
437			ConstQuaternionMap quat(q_joint.derived().data());
438			//assert(math::fabs(quat.coeffs().squaredNorm()-1.) <= sqrt(Eigen::NumTraits<typename V::Scalar>::epsilon())); TODO: check validity of the rhs precision
439			assert(math::fabs(static_cast<Scalar>(quat.coeffs().squaredNorm()-1)) <= 1e-4);
440
441			M.rotation(quat.matrix());
442			M.translation().setZero();
443			}
444
445			template<typename QuaternionDerived>
446		6319	void calc(JointDataDerived & data,
447			const typename Eigen::QuaternionBase<QuaternionDerived> & quat) const
448			{
449	✓✗	6319	data.M.rotation(quat.matrix());
450		6319	}
451
452			template<typename ConfigVector>
453			EIGEN_DONT_INLINE
454		6331	void calc(JointDataDerived & data,
455			const typename Eigen::PlainObjectBase<ConfigVector> & qs) const
456			{
457			typedef typename Eigen::Quaternion<typename ConfigVector::Scalar,ConfigVector::Options> Quaternion;
458			typedef Eigen::Map<const Quaternion> ConstQuaternionMap;
459
460	✓✗✓✗ ✓✗	6331	ConstQuaternionMap quat(qs.template segment<NQ>(idx_q()).data());
461	✓✗	6331	calc(data,quat);
462		6331	}
463
464			template<typename ConfigVector>
465			EIGEN_DONT_INLINE
466			void calc(JointDataDerived & data,
467			const typename Eigen::MatrixBase<ConfigVector> & qs) const
468			{
469			typedef typename Eigen::Quaternion<Scalar,Options> Quaternion;
470
471			const Quaternion quat(qs.template segment<NQ>(idx_q()));
472			calc(data,quat);
473			}
474
475			template<typename ConfigVector, typename TangentVector>
476		1182	void calc(JointDataDerived & data,
477			const typename Eigen::MatrixBase<ConfigVector> & qs,
478			const typename Eigen::MatrixBase<TangentVector> & vs) const
479			{
480		1182	calc(data,qs.derived());
481
482	✓✗	1182	data.v.angular() = vs.template segment<NV>(idx_v());
483		1182	}
484
485			template<typename Matrix6Like>
486		8	void calc_aba(JointDataDerived & data,
487			const Eigen::MatrixBase<Matrix6Like> & I,
488			const bool update_I) const
489			{
490	✓✗	8	data.U = I.template block<6,3>(0,Inertia::ANGULAR);
491
492			// compute inverse
493			// data.Dinv.setIdentity();
494			// data.U.template middleRows<3>(Inertia::ANGULAR).llt().solveInPlace(data.Dinv);
495	✓✗	8	internal::PerformStYSInversion<Scalar>::run(data.U.template middleRows<3>(Inertia::ANGULAR),data.Dinv);
496
497	✓✗	8	data.UDinv.template middleRows<3>(Inertia::ANGULAR).setIdentity(); // can be put in data constructor
498	✓✗✓✗ ✓✗✓✗	8	data.UDinv.template middleRows<3>(Inertia::LINEAR).noalias() = data.U.template block<3,3>(Inertia::LINEAR, 0) * data.Dinv;
499
500	✓✓	8	if (update_I)
501			{
502		2	Matrix6Like & I_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like,I);
503			I_.template block<3,3>(Inertia::LINEAR,Inertia::LINEAR)
504	✓✗✓✗ ✓✗✓✗	2	-= data.UDinv.template middleRows<3>(Inertia::LINEAR) * I_.template block<3,3> (Inertia::ANGULAR, Inertia::LINEAR);
505	✓✗	2	I_.template block<6,3>(0,Inertia::ANGULAR).setZero();
506	✓✗	2	I_.template block<3,3>(Inertia::ANGULAR,Inertia::LINEAR).setZero();
507			}
508		8	}
509
510	✓✗	15368	static std::string classname() { return std::string("JointModelSpherical"); }
511		15326	std::string shortname() const { return classname(); }
512
513			/// \returns An expression of *this with the Scalar type casted to NewScalar.
514			template<typename NewScalar>
515		4	JointModelSphericalTpl<NewScalar,Options> cast() const
516			{
517			typedef JointModelSphericalTpl<NewScalar,Options> ReturnType;
518		4	ReturnType res;
519		4	res.setIndexes(id(),idx_q(),idx_v());
520		4	return res;
521			}
522
523			}; // struct JointModelSphericalTpl
524
525			} // namespace pinocchio
526
527			#include <boost/type_traits.hpp>
528
529			namespace boost
530			{
531			template<typename Scalar, int Options>
532			struct has_nothrow_constructor< ::pinocchio::JointModelSphericalTpl<Scalar,Options> >
533			: public integral_constant<bool,true> {};
534
535			template<typename Scalar, int Options>
536			struct has_nothrow_copy< ::pinocchio::JointModelSphericalTpl<Scalar,Options> >
537			: public integral_constant<bool,true> {};
538
539			template<typename Scalar, int Options>
540			struct has_nothrow_constructor< ::pinocchio::JointDataSphericalTpl<Scalar,Options> >
541			: public integral_constant<bool,true> {};
542
543			template<typename Scalar, int Options>
544			struct has_nothrow_copy< ::pinocchio::JointDataSphericalTpl<Scalar,Options> >
545			: public integral_constant<bool,true> {};
546			}
547
548			#endif // ifndef __pinocchio_joint_spherical_hpp__