Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	include/pinocchio/multibody/joint/joint-translation.hpp	Lines:	133	146	91.1 %
Date:	2024-01-23 21:41:47	Branches:	60	128	46.9 %


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

#ifndef __pinocchio_joint_translation_hpp__
#define __pinocchio_joint_translation_hpp__

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

namespace pinocchio
{

  template<typename Scalar, int Options=0> struct MotionTranslationTpl;
  typedef MotionTranslationTpl<double> MotionTranslation;

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

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

  template<typename _Scalar, int _Options>
  struct traits< MotionTranslationTpl<_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 MotionTranslationTpl

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

    MOTION_TYPEDEF_TPL(MotionTranslationTpl);

    MotionTranslationTpl() {}

    template<typename Vector3Like>
    MotionTranslationTpl(const Eigen::MatrixBase<Vector3Like> & v)
    : m_v(v)
    {}

    MotionTranslationTpl(const MotionTranslationTpl & other)
    : m_v(other.m_v)
    {}

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

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

    bool isEqual_impl(const MotionTranslationTpl & other) const
    {
      return m_v == other.m_v;
    }

    MotionTranslationTpl & operator=(const MotionTranslationTpl & other)
    {
      m_v = other.m_v;
      return *this;
    }

    template<typename Derived>
    void addTo(MotionDense<Derived> & other) const
    {
      other.linear() += m_v;
    }

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

    template<typename S2, int O2, typename D2>
    void se3Action_impl(const SE3Tpl<S2,O2> & m, MotionDense<D2> & v) const
    {
      v.angular().setZero();
      v.linear().noalias() = m.rotation() * m_v; // TODO: check efficiency
    }

    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
      v.linear().noalias() = m.rotation().transpose() * m_v;

      // Angular
      v.angular().setZero();
    }

    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.angular().cross(m_v);

      // Angular
      mout.angular().setZero();
    }

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

    const Vector3 & linear() const { return m_v; }
    Vector3 & linear() { return m_v; }

  protected:

    Vector3 m_v;

  }; // struct MotionTranslationTpl

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

  template<typename Scalar, int Options> struct TransformTranslationTpl;

  template<typename _Scalar, int _Options>
  struct traits< TransformTranslationTpl<_Scalar,_Options> >
  {
    enum {
      Options = _Options,
      LINEAR = 0,
      ANGULAR = 3
    };
    typedef _Scalar Scalar;
    typedef SE3Tpl<Scalar,Options> PlainType;
    typedef Eigen::Matrix<Scalar,3,1,Options> Vector3;
    typedef Eigen::Matrix<Scalar,3,3,Options> Matrix3;
    typedef typename Matrix3::IdentityReturnType AngularType;
    typedef AngularType AngularRef;
    typedef AngularType ConstAngularRef;
    typedef Vector3 LinearType;
    typedef LinearType & LinearRef;
    typedef const LinearType & ConstLinearRef;
    typedef typename traits<PlainType>::ActionMatrixType ActionMatrixType;
    typedef typename traits<PlainType>::HomogeneousMatrixType HomogeneousMatrixType;
  }; // traits TransformTranslationTpl

  template<typename Scalar, int Options>
  struct SE3GroupAction< TransformTranslationTpl<Scalar,Options> >
  { typedef typename traits <TransformTranslationTpl<Scalar,Options> >::PlainType ReturnType; };

  template<typename _Scalar, int _Options>
  struct TransformTranslationTpl
  : SE3Base< TransformTranslationTpl<_Scalar,_Options> >
  {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    PINOCCHIO_SE3_TYPEDEF_TPL(TransformTranslationTpl);
    typedef typename traits<TransformTranslationTpl>::Vector3 Vector3;

    TransformTranslationTpl() {}

    template<typename Vector3Like>
    TransformTranslationTpl(const Eigen::MatrixBase<Vector3Like> & translation)
    : m_translation(translation)
    {}

    PlainType plain() const
    {
      PlainType res(PlainType::Identity());
      res.rotation().setIdentity();
      res.translation() = translation();

      return res;
    }

    operator PlainType() const { return plain(); }

    template<typename S2, int O2>
    typename SE3GroupAction<TransformTranslationTpl>::ReturnType
    se3action(const SE3Tpl<S2,O2> & m) const
    {
      typedef typename SE3GroupAction<TransformTranslationTpl>::ReturnType ReturnType;
      ReturnType res(m);
      res.translation() += translation();

      return res;
    }

    ConstLinearRef translation() const { return m_translation; }
    LinearRef translation() { return m_translation; }

    AngularType rotation() const { return AngularType(3,3); }

    bool isEqual(const TransformTranslationTpl & other) const
    {
      return m_translation == other.m_translation;
    }

  protected:

    LinearType m_translation;
  };

  template<typename Scalar, int Options> struct ConstraintTranslationTpl;

  template<typename _Scalar, int _Options>
  struct traits< ConstraintTranslationTpl<_Scalar,_Options> >
  {
    typedef _Scalar Scalar;

    enum { Options = _Options };
    enum { LINEAR = 0, ANGULAR = 3 };

    typedef MotionTranslationTpl<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;
  }; // traits ConstraintTranslationTpl

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

    PINOCCHIO_CONSTRAINT_TYPEDEF_TPL(ConstraintTranslationTpl)

    enum { NV = 3 };

    ConstraintTranslationTpl() {}

//    template<typename S1, int O1>
//    Motion operator*(const MotionTranslationTpl<S1,O1> & vj) const
//    { return Motion(vj(), Motion::Vector3::Zero()); }

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

    int nv_impl() const { return NV; }

    struct ConstraintTranspose
    {
      const ConstraintTranslationTpl & ref;
      ConstraintTranspose(const ConstraintTranslationTpl & ref) : ref(ref) {}

      template<typename Derived>
      typename ForceDense<Derived>::ConstLinearType
      operator* (const ForceDense<Derived> & phi)
      {
        return phi.linear();
      }

      /* [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>(LINEAR);
      }

    }; // struct ConstraintTranspose

    ConstraintTranspose transpose () const { return ConstraintTranspose(*this); }

    DenseBase matrix_impl() const
    {
      DenseBase S;
      S.template middleRows<3>(LINEAR).setIdentity();
      S.template middleRows<3>(ANGULAR).setZero();
      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> M;
      M.template middleRows<3>(LINEAR) = m.rotation();
      M.template middleRows<3>(ANGULAR).setZero();

      return M;
    }

    template<typename S1, int O1>
    Eigen::Matrix<S1,6,3,O1> se3ActionInverse(const SE3Tpl<S1,O1> & m) const
    {
      Eigen::Matrix<S1,6,3,O1> M;
      M.template middleRows<3>(LINEAR) = m.rotation().transpose();
      M.template middleRows<3>(ANGULAR).setZero();

      return M;
    }

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

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

      return res;
    }

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

  }; // struct ConstraintTranslationTpl

  template<typename MotionDerived, typename S2, int O2>
  inline typename MotionDerived::MotionPlain
  operator^(const MotionDense<MotionDerived> & m1,
            const MotionTranslationTpl<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 ConstraintTranslationTpl<S2,O2> &)
  {
    typedef ConstraintTranslationTpl<S2,O2> Constraint;
    Eigen::Matrix<S2,6,3,O2> M;
    alphaSkew(Y.mass(),Y.lever(),M.template middleRows<3>(Constraint::ANGULAR));
    M.template middleRows<3>(Constraint::LINEAR).setZero();
    M.template middleRows<3>(Constraint::LINEAR).diagonal().fill(Y.mass ());

    return M;
  }

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

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

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

  template<typename Scalar, int Options> struct JointTranslationTpl;

  template<typename _Scalar, int _Options>
  struct traits< JointTranslationTpl<_Scalar,_Options> >
  {
    enum {
      NQ = 3,
      NV = 3
    };
    typedef _Scalar Scalar;
    enum { Options = _Options };
    typedef JointDataTranslationTpl<Scalar,Options> JointDataDerived;
    typedef JointModelTranslationTpl<Scalar,Options> JointModelDerived;
    typedef ConstraintTranslationTpl<Scalar,Options> Constraint_t;
    typedef TransformTranslationTpl<Scalar,Options> Transformation_t;
    typedef MotionTranslationTpl<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;
  }; // traits JointTranslationTpl

  template<typename Scalar, int Options>
  struct traits< JointDataTranslationTpl<Scalar,Options> >
  { typedef JointTranslationTpl<Scalar,Options> JointDerived; };

  template<typename Scalar, int Options>
  struct traits< JointModelTranslationTpl<Scalar,Options> >
  { typedef JointTranslationTpl<Scalar,Options> JointDerived; };

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

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

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

    static std::string classname() { return std::string("JointDataTranslation"); }
    std::string shortname() const { return classname(); }
  }; // struct JointDataTranslationTpl

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

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

    typedef JointModelBase<JointModelTranslationTpl> 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};
    }

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

    template<typename ConfigVector>
    void calc(JointDataDerived & data,
              const typename Eigen::MatrixBase<ConfigVector> & qs) const
    {
      data.M.translation() = this->jointConfigSelector(qs);
    }

    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.linear() = this->jointVelocitySelector(vs);
    }

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

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

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

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

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

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

  }; // struct JointModelTranslationTpl

} // namespace pinocchio

#include <boost/type_traits.hpp>

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

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

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

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

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