Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	include/pinocchio/multibody/constraint-generic.hpp	Lines:	37	38	97.4 %
Date:	2024-01-23 21:41:47	Branches:	15	32	46.9 %


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

#ifndef __pinocchio_multibody_constraint_generic_hpp__
#define __pinocchio_multibody_constraint_generic_hpp__

namespace pinocchio
{

  template<int _Dim, typename _Scalar, int _Options>
  struct traits< ConstraintTpl<_Dim, _Scalar, _Options> >
  {
    typedef _Scalar Scalar;
    enum
    {
      LINEAR = 0,
      ANGULAR = 3,
      Options = _Options,
      Dim = _Dim
    };

    typedef MotionTpl<Scalar,Options> JointMotion;
    typedef Eigen::Matrix<Scalar,Dim,1,Options> JointForce;
    typedef Eigen::Matrix<Scalar,6,Dim,Options> DenseBase;

    typedef typename PINOCCHIO_EIGEN_REF_CONST_TYPE(DenseBase) ConstMatrixReturnType;
    typedef typename PINOCCHIO_EIGEN_REF_TYPE(DenseBase) MatrixReturnType;

  }; // traits ConstraintTpl

  template<int Dim, typename Scalar, int Options>
  struct SE3GroupAction< ConstraintTpl<Dim,Scalar,Options> >
  { typedef Eigen::Matrix<Scalar,6,Dim> ReturnType; };

  template<int Dim, typename Scalar, int Options, typename MotionDerived>
  struct MotionAlgebraAction< ConstraintTpl<Dim,Scalar,Options>, MotionDerived >
  { typedef Eigen::Matrix<Scalar,6,Dim> ReturnType; };

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

    typedef ConstraintBase<ConstraintTpl> Base;

    friend class ConstraintBase<ConstraintTpl>;
    PINOCCHIO_CONSTRAINT_TYPEDEF_TPL(ConstraintTpl)

    enum { NV = _Dim };

    using Base::nv;

    template<typename D>
    explicit ConstraintTpl(const Eigen::MatrixBase<D> & _S) : S(_S)
    {
      // There is currently a bug in Eigen/Core/util/StaticAssert.h in the use of the full namespace path
      // TODO
      EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(DenseBase, D);
    }

    ConstraintTpl() : S()
    {
      EIGEN_STATIC_ASSERT(_Dim!=Eigen::Dynamic,
                          YOU_CALLED_A_DYNAMIC_SIZE_METHOD_ON_A_FIXED_SIZE_MATRIX_OR_VECTOR)
    }

    // It is only valid for dynamics size
    explicit ConstraintTpl(const int dim) : S(6,dim)
    {
      EIGEN_STATIC_ASSERT(_Dim==Eigen::Dynamic,
                          YOU_CALLED_A_FIXED_SIZE_METHOD_ON_A_DYNAMIC_SIZE_MATRIX_OR_VECTOR)
    }

    static ConstraintTpl Zero(const int dim)
    {
      return ConstraintTpl(dim);
    }

    template<typename VectorLike>
    JointMotion __mult__(const Eigen::MatrixBase<VectorLike> & vj) const
    {
      return JointMotion(S*vj);
    }

    struct Transpose
    {
      const ConstraintTpl & ref;
      Transpose( const ConstraintTpl & ref ) : ref(ref) {}

      template<typename Derived>
      JointForce operator*(const ForceDense<Derived> & f) const
      { return (ref.S.transpose()*f.toVector()).eval(); }

      template<typename D>
      typename Eigen::Matrix<Scalar,NV,Eigen::Dynamic>
      operator*(const Eigen::MatrixBase<D> & F)
      {
        return (ref.S.transpose()*F).eval();
      }

    };

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

    MatrixReturnType matrix_impl() { return S; }
    ConstMatrixReturnType matrix_impl() const { return S; }

    int nv_impl() const { return (int)S.cols(); }

    template<typename S2,int O2>
    friend typename ConstraintTpl<_Dim,_Scalar,_Options>::DenseBase
    operator*(const InertiaTpl<S2,O2> & Y, const ConstraintTpl & S)
    {
      typedef typename ConstraintTpl::DenseBase ReturnType;
      ReturnType res(6,S.nv());
      motionSet::inertiaAction(Y,S.S,res);
      return res;
    }

    template<typename S2,int O2>
    friend Eigen::Matrix<_Scalar,6,_Dim>
    operator*(const Eigen::Matrix<S2,6,6,O2> & Ymatrix, const ConstraintTpl & S)
    {
      typedef Eigen::Matrix<_Scalar,6,_Dim> ReturnType;
      return ReturnType(Ymatrix*S.matrix());

    }

    DenseBase se3Action(const SE3Tpl<Scalar,Options> & m) const
    {
      DenseBase res(6,nv());
      motionSet::se3Action(m,S,res);
      return res;
    }

    DenseBase se3ActionInverse(const SE3Tpl<Scalar,Options> & m) const
    {
      DenseBase res(6,nv());
      motionSet::se3ActionInverse(m,S,res);
      return res;
    }

    template<typename MotionDerived>
    DenseBase motionAction(const MotionDense<MotionDerived> & v) const
    {
      DenseBase res(6,nv());
      motionSet::motionAction(v,S,res);
      return res;
    }

    void disp_impl(std::ostream & os) const { os << "S =\n" << S << std::endl;}

    bool isEqual(const ConstraintTpl & other) const
    {
      return S == other.S;
    }

  protected:
    DenseBase S;
  }; // class ConstraintTpl


} // namespace pinocchio

#endif // ifndef __pinocchio_multibody_constraint_generic_hpp__



Generated by: GCOVR (Version 4.2)

Line	Branch	Exec	Source
1			//
2			// Copyright (c) 2015-2019 CNRS, INRIA
3			// Copyright (c) 2016 Wandercraft, 86 rue de Paris 91400 Orsay, France.
4			//
5
6			#ifndef __pinocchio_multibody_constraint_generic_hpp__
7			#define __pinocchio_multibody_constraint_generic_hpp__
8
9			namespace pinocchio
10			{
11
12			template<int _Dim, typename _Scalar, int _Options>
13			struct traits< ConstraintTpl<_Dim, _Scalar, _Options> >
14			{
15			typedef _Scalar Scalar;
16			enum
17			{
18			LINEAR = 0,
19			ANGULAR = 3,
20			Options = _Options,
21			Dim = _Dim
22			};
23
24			typedef MotionTpl<Scalar,Options> JointMotion;
25			typedef Eigen::Matrix<Scalar,Dim,1,Options> JointForce;
26			typedef Eigen::Matrix<Scalar,6,Dim,Options> DenseBase;
27
28			typedef typename PINOCCHIO_EIGEN_REF_CONST_TYPE(DenseBase) ConstMatrixReturnType;
29			typedef typename PINOCCHIO_EIGEN_REF_TYPE(DenseBase) MatrixReturnType;
30
31			}; // traits ConstraintTpl
32
33			template<int Dim, typename Scalar, int Options>
34			struct SE3GroupAction< ConstraintTpl<Dim,Scalar,Options> >
35			{ typedef Eigen::Matrix<Scalar,6,Dim> ReturnType; };
36
37			template<int Dim, typename Scalar, int Options, typename MotionDerived>
38			struct MotionAlgebraAction< ConstraintTpl<Dim,Scalar,Options>, MotionDerived >
39			{ typedef Eigen::Matrix<Scalar,6,Dim> ReturnType; };
40
41			template<int _Dim, typename _Scalar, int _Options>
42			struct ConstraintTpl
43			: public ConstraintBase< ConstraintTpl<_Dim,_Scalar,_Options> >
44			{
45			EIGEN_MAKE_ALIGNED_OPERATOR_NEW
46
47			typedef ConstraintBase<ConstraintTpl> Base;
48
49			friend class ConstraintBase<ConstraintTpl>;
50			PINOCCHIO_CONSTRAINT_TYPEDEF_TPL(ConstraintTpl)
51
52			enum { NV = _Dim };
53
54			using Base::nv;
55
56			template<typename D>
57		1552	explicit ConstraintTpl(const Eigen::MatrixBase<D> & _S) : S(_S)
58			{
59			// There is currently a bug in Eigen/Core/util/StaticAssert.h in the use of the full namespace path
60			// TODO
61			EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(DenseBase, D);
62		1552	}
63
64			ConstraintTpl() : S()
65			{
66			EIGEN_STATIC_ASSERT(_Dim!=Eigen::Dynamic,
67			YOU_CALLED_A_DYNAMIC_SIZE_METHOD_ON_A_FIXED_SIZE_MATRIX_OR_VECTOR)
68			}
69
70			// It is only valid for dynamics size
71	✓✗	71	explicit ConstraintTpl(const int dim) : S(6,dim)
72			{
73			EIGEN_STATIC_ASSERT(_Dim==Eigen::Dynamic,
74			YOU_CALLED_A_FIXED_SIZE_METHOD_ON_A_DYNAMIC_SIZE_MATRIX_OR_VECTOR)
75		71	}
76
77		45	static ConstraintTpl Zero(const int dim)
78			{
79		45	return ConstraintTpl(dim);
80			}
81
82			template<typename VectorLike>
83		2	JointMotion __mult__(const Eigen::MatrixBase<VectorLike> & vj) const
84			{
85	✓✗	3	return JointMotion(S*vj);
86			}
87
88			struct Transpose
89			{
90			const ConstraintTpl & ref;
91		2	Transpose( const ConstraintTpl & ref ) : ref(ref) {}
92
93			template<typename Derived>
94		1	JointForce operator*(const ForceDense<Derived> & f) const
95	✓✗✓✗	2	{ return (ref.S.transpose()*f.toVector()).eval(); }
96
97			template<typename D>
98			typename Eigen::Matrix<Scalar,NV,Eigen::Dynamic>
99		1	operator*(const Eigen::MatrixBase<D> & F)
100			{
101	✓✗✓✗ ✗✗	2	return (ref.S.transpose()*F).eval();
102			}
103
104			};
105
106		2	Transpose transpose() const { return Transpose(*this); }
107
108		270	MatrixReturnType matrix_impl() { return S; }
109		3	ConstMatrixReturnType matrix_impl() const { return S; }
110
111		27	int nv_impl() const { return (int)S.cols(); }
112
113			template<typename S2,int O2>
114			friend typename ConstraintTpl<_Dim,_Scalar,_Options>::DenseBase
115		1	operator*(const InertiaTpl<S2,O2> & Y, const ConstraintTpl & S)
116			{
117			typedef typename ConstraintTpl::DenseBase ReturnType;
118	✓✗	1	ReturnType res(6,S.nv());
119	✓✗	1	motionSet::inertiaAction(Y,S.S,res);
120		1	return res;
121			}
122
123			template<typename S2,int O2>
124			friend Eigen::Matrix<_Scalar,6,_Dim>
125		1	operator*(const Eigen::Matrix<S2,6,6,O2> & Ymatrix, const ConstraintTpl & S)
126			{
127			typedef Eigen::Matrix<_Scalar,6,_Dim> ReturnType;
128	✓✗	2	return ReturnType(Ymatrix*S.matrix());
129
130			}
131
132		3	DenseBase se3Action(const SE3Tpl<Scalar,Options> & m) const
133			{
134	✓✗	3	DenseBase res(6,nv());
135	✓✗	3	motionSet::se3Action(m,S,res);
136		3	return res;
137			}
138
139		3	DenseBase se3ActionInverse(const SE3Tpl<Scalar,Options> & m) const
140			{
141	✓✗	3	DenseBase res(6,nv());
142	✓✗	3	motionSet::se3ActionInverse(m,S,res);
143		3	return res;
144			}
145
146			template<typename MotionDerived>
147		1	DenseBase motionAction(const MotionDense<MotionDerived> & v) const
148			{
149	✓✗	1	DenseBase res(6,nv());
150	✓✗	1	motionSet::motionAction(v,S,res);
151		1	return res;
152			}
153
154			void disp_impl(std::ostream & os) const { os << "S =\n" << S << std::endl;}
155
156		386	bool isEqual(const ConstraintTpl & other) const
157			{
158		386	return S == other.S;
159			}
160
161			protected:
162			DenseBase S;
163			}; // class ConstraintTpl
164
165
166			} // namespace pinocchio
167
168			#endif // ifndef __pinocchio_multibody_constraint_generic_hpp__
169