Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	include/pinocchio/multibody/joint/joint-revolute-unbounded.hpp	Lines:	46	46	100.0 %
Date:	2024-01-23 21:41:47	Branches:	20	38	52.6 %


//
// Copyright (c) 2016-2019 CNRS INRIA
//

#ifndef __pinocchio_joint_revolute_unbounded_hpp__
#define __pinocchio_joint_revolute_unbounded_hpp__

#include "pinocchio/math/fwd.hpp"
#include "pinocchio/math/sincos.hpp"
#include "pinocchio/spatial/inertia.hpp"
#include "pinocchio/multibody/joint/joint-base.hpp"
#include "pinocchio/multibody/joint/joint-revolute.hpp"

namespace pinocchio
{

  template<typename Scalar, int Options, int axis> struct JointRevoluteUnboundedTpl;

  template<typename _Scalar, int _Options, int axis>
  struct traits< JointRevoluteUnboundedTpl<_Scalar,_Options,axis> >
  {
    enum {
      NQ = 2,
      NV = 1
    };
    typedef _Scalar Scalar;
    enum { Options = _Options };
    typedef JointDataRevoluteUnboundedTpl<Scalar,Options,axis> JointDataDerived;
    typedef JointModelRevoluteUnboundedTpl<Scalar,Options,axis> JointModelDerived;
    typedef ConstraintRevoluteTpl<Scalar,Options,axis> Constraint_t;
    typedef TransformRevoluteTpl<Scalar,Options,axis> Transformation_t;
    typedef MotionRevoluteTpl<Scalar,Options,axis> 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, int axis>
  struct traits< JointDataRevoluteUnboundedTpl<Scalar,Options,axis> >
  { typedef JointRevoluteUnboundedTpl<Scalar,Options,axis> JointDerived; };

  template<typename Scalar, int Options, int axis>
  struct traits< JointModelRevoluteUnboundedTpl<Scalar,Options,axis> >
  { typedef JointRevoluteUnboundedTpl<Scalar,Options,axis> JointDerived; };

  template<typename _Scalar, int _Options, int axis>
  struct JointDataRevoluteUnboundedTpl : public JointDataBase< JointDataRevoluteUnboundedTpl<_Scalar,_Options,axis> >
  {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    typedef JointRevoluteUnboundedTpl<_Scalar,_Options,axis> 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;

    JointDataRevoluteUnboundedTpl()
    : M((Scalar)0,(Scalar)1)
    , v((Scalar)0)
    , U(U_t::Zero())
    , Dinv(D_t::Zero())
    , UDinv(UD_t::Zero())
    {}

    static std::string classname()
    {
      return std::string("JointDataRUB") + axisLabel<axis>();
    }
    std::string shortname() const { return classname(); }

  }; // struct JointDataRevoluteUnbounded

  template<typename NewScalar, typename Scalar, int Options, int axis>
  struct CastType< NewScalar, JointModelRevoluteUnboundedTpl<Scalar,Options,axis> >
  {
    typedef JointModelRevoluteUnboundedTpl<NewScalar,Options,axis> type;
  };

  template<typename _Scalar, int _Options, int axis>
  struct JointModelRevoluteUnboundedTpl
  : public JointModelBase< JointModelRevoluteUnboundedTpl<_Scalar,_Options,axis> >
  {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    typedef JointRevoluteUnboundedTpl<_Scalar,_Options,axis> JointDerived;
    PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived);
    typedef JointRevoluteTpl<Scalar,_Options,axis> JointDerivedBase;

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

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

    template<typename ConfigVector>
    void calc(JointDataDerived & data,
              const typename Eigen::MatrixBase<ConfigVector> & qs) const
    {
      typedef typename ConfigVector::Scalar OtherScalar;
      typename ConfigVector::template ConstFixedSegmentReturnType<NQ>::Type
      & q = qs.template segment<NQ> (idx_q());

      const OtherScalar & ca = q(0);
      const OtherScalar & sa = q(1);

      data.M.setValues(sa,ca);
    }

    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.angularRate() = static_cast<Scalar>(vs[idx_v()]);
    }

    template<typename Matrix6Like>
    void calc_aba(JointDataDerived & data, const Eigen::MatrixBase<Matrix6Like> & I, const bool update_I) const
    {
      data.U = I.col(Inertia::ANGULAR + axis);
      data.Dinv[0] = (Scalar)(1)/I(Inertia::ANGULAR + axis,Inertia::ANGULAR + axis);
      data.UDinv.noalias() = data.U * data.Dinv[0];

      if (update_I)
        PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like,I) -= data.UDinv * data.U.transpose();
    }

    static std::string classname()
    {
      return std::string("JointModelRUB") + axisLabel<axis>();
    }
    std::string shortname() const { return classname(); }

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

  }; // struct JointModelRevoluteUnboundedTpl

  struct UnboundedRevoluteAffineTransform
  {
    template<typename ConfigVectorIn, typename Scalar, typename ConfigVectorOut>
    static void run(const Eigen::MatrixBase<ConfigVectorIn> & q,
                    const Scalar & scaling,
                    const Scalar & offset,
                    const Eigen::MatrixBase<ConfigVectorOut> & dest)
    {
      EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(ConfigVectorIn,2);
      EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(ConfigVectorOut,2);

      const typename ConfigVectorIn::Scalar & ca = q(0);
      const typename ConfigVectorIn::Scalar & sa = q(1);

      const typename ConfigVectorIn::Scalar & theta = math::atan2(sa,ca);
      const typename ConfigVectorIn::Scalar & theta_transform = scaling * theta + offset;

      ConfigVectorOut & dest_ = PINOCCHIO_EIGEN_CONST_CAST(ConfigVectorOut,dest);
      SINCOS(theta_transform,&dest_.coeffRef(1),&dest_.coeffRef(0));
    }
  };

  template<typename Scalar, int Options, int axis>
  struct ConfigVectorAffineTransform< JointRevoluteUnboundedTpl<Scalar,Options,axis> >
  {
    typedef UnboundedRevoluteAffineTransform Type;
  };

  typedef JointRevoluteUnboundedTpl<double,0,0> JointRUBX;
  typedef JointDataRevoluteUnboundedTpl<double,0,0> JointDataRUBX;
  typedef JointModelRevoluteUnboundedTpl<double,0,0> JointModelRUBX;

  typedef JointRevoluteUnboundedTpl<double,0,1> JointRUBY;
  typedef JointDataRevoluteUnboundedTpl<double,0,1> JointDataRUBY;
  typedef JointModelRevoluteUnboundedTpl<double,0,1> JointModelRUBY;

  typedef JointRevoluteUnboundedTpl<double,0,2> JointRUBZ;
  typedef JointDataRevoluteUnboundedTpl<double,0,2> JointDataRUBZ;
  typedef JointModelRevoluteUnboundedTpl<double,0,2> JointModelRUBZ;

} //namespace pinocchio

#include <boost/type_traits.hpp>

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

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

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

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

#endif // ifndef __pinocchio_joint_revolute_unbounded_hpp__


Generated by: GCOVR (Version 4.2)

Line	Branch	Exec	Source
1			//
2			// Copyright (c) 2016-2019 CNRS INRIA
3			//
4
5			#ifndef __pinocchio_joint_revolute_unbounded_hpp__
6			#define __pinocchio_joint_revolute_unbounded_hpp__
7
8			#include "pinocchio/math/fwd.hpp"
9			#include "pinocchio/math/sincos.hpp"
10			#include "pinocchio/spatial/inertia.hpp"
11			#include "pinocchio/multibody/joint/joint-base.hpp"
12			#include "pinocchio/multibody/joint/joint-revolute.hpp"
13
14			namespace pinocchio
15			{
16
17			template<typename Scalar, int Options, int axis> struct JointRevoluteUnboundedTpl;
18
19			template<typename _Scalar, int _Options, int axis>
20			struct traits< JointRevoluteUnboundedTpl<_Scalar,_Options,axis> >
21			{
22			enum {
23			NQ = 2,
24			NV = 1
25			};
26			typedef _Scalar Scalar;
27			enum { Options = _Options };
28			typedef JointDataRevoluteUnboundedTpl<Scalar,Options,axis> JointDataDerived;
29			typedef JointModelRevoluteUnboundedTpl<Scalar,Options,axis> JointModelDerived;
30			typedef ConstraintRevoluteTpl<Scalar,Options,axis> Constraint_t;
31			typedef TransformRevoluteTpl<Scalar,Options,axis> Transformation_t;
32			typedef MotionRevoluteTpl<Scalar,Options,axis> Motion_t;
33			typedef MotionZeroTpl<Scalar,Options> Bias_t;
34
35			// [ABA]
36			typedef Eigen::Matrix<Scalar,6,NV,Options> U_t;
37			typedef Eigen::Matrix<Scalar,NV,NV,Options> D_t;
38			typedef Eigen::Matrix<Scalar,6,NV,Options> UD_t;
39
40			PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE
41
42			typedef Eigen::Matrix<Scalar,NQ,1,Options> ConfigVector_t;
43			typedef Eigen::Matrix<Scalar,NV,1,Options> TangentVector_t;
44			};
45
46			template<typename Scalar, int Options, int axis>
47			struct traits< JointDataRevoluteUnboundedTpl<Scalar,Options,axis> >
48			{ typedef JointRevoluteUnboundedTpl<Scalar,Options,axis> JointDerived; };
49
50			template<typename Scalar, int Options, int axis>
51			struct traits< JointModelRevoluteUnboundedTpl<Scalar,Options,axis> >
52			{ typedef JointRevoluteUnboundedTpl<Scalar,Options,axis> JointDerived; };
53
54			template<typename _Scalar, int _Options, int axis>
55			struct JointDataRevoluteUnboundedTpl : public JointDataBase< JointDataRevoluteUnboundedTpl<_Scalar,_Options,axis> >
56			{
57		98	EIGEN_MAKE_ALIGNED_OPERATOR_NEW
58			typedef JointRevoluteUnboundedTpl<_Scalar,_Options,axis> JointDerived;
59			PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(JointDerived);
60		1944	PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR
61
62			Constraint_t S;
63			Transformation_t M;
64			Motion_t v;
65			Bias_t c;
66
67			// [ABA] specific data
68			U_t U;
69			D_t Dinv;
70			UD_t UDinv;
71
72		18742	JointDataRevoluteUnboundedTpl()
73			: M((Scalar)0,(Scalar)1)
74			, v((Scalar)0)
75			, U(U_t::Zero())
76			, Dinv(D_t::Zero())
77	✓✗✓✗ ✓✗	18742	, UDinv(UD_t::Zero())
78		18742	{}
79
80		264	static std::string classname()
81			{
82	✓✗✓✗	264	return std::string("JointDataRUB") + axisLabel<axis>();
83			}
84		9	std::string shortname() const { return classname(); }
85
86			}; // struct JointDataRevoluteUnbounded
87
88			template<typename NewScalar, typename Scalar, int Options, int axis>
89			struct CastType< NewScalar, JointModelRevoluteUnboundedTpl<Scalar,Options,axis> >
90			{
91			typedef JointModelRevoluteUnboundedTpl<NewScalar,Options,axis> type;
92			};
93
94			template<typename _Scalar, int _Options, int axis>
95			struct JointModelRevoluteUnboundedTpl
96			: public JointModelBase< JointModelRevoluteUnboundedTpl<_Scalar,_Options,axis> >
97			{
98		120	EIGEN_MAKE_ALIGNED_OPERATOR_NEW
99			typedef JointRevoluteUnboundedTpl<_Scalar,_Options,axis> JointDerived;
100			PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived);
101			typedef JointRevoluteTpl<Scalar,_Options,axis> JointDerivedBase;
102
103			typedef JointModelBase<JointModelRevoluteUnboundedTpl> Base;
104			using Base::id;
105			using Base::idx_q;
106			using Base::idx_v;
107			using Base::setIndexes;
108
109		18586	JointDataDerived createData() const { return JointDataDerived(); }
110
111		2	const std::vector<bool> hasConfigurationLimit() const
112			{
113	✓✗	2	return {false, false};
114			}
115
116		2	const std::vector<bool> hasConfigurationLimitInTangent() const
117			{
118	✓✗	2	return {false};
119			}
120
121			template<typename ConfigVector>
122		36864	void calc(JointDataDerived & data,
123			const typename Eigen::MatrixBase<ConfigVector> & qs) const
124			{
125			typedef typename ConfigVector::Scalar OtherScalar;
126			typename ConfigVector::template ConstFixedSegmentReturnType<NQ>::Type
127	✓✗✓✗	36864	& q = qs.template segment<NQ> (idx_q());
128
129	✓✗	36864	const OtherScalar & ca = q(0);
130	✓✗	36864	const OtherScalar & sa = q(1);
131
132		36864	data.M.setValues(sa,ca);
133		36864	}
134
135			template<typename ConfigVector, typename TangentVector>
136		6202	void calc(JointDataDerived & data,
137			const typename Eigen::MatrixBase<ConfigVector> & qs,
138			const typename Eigen::MatrixBase<TangentVector> & vs) const
139			{
140		6202	calc(data,qs.derived());
141
142		6202	data.v.angularRate() = static_cast<Scalar>(vs[idx_v()]);
143		6202	}
144
145			template<typename Matrix6Like>
146		46	void calc_aba(JointDataDerived & data, const Eigen::MatrixBase<Matrix6Like> & I, const bool update_I) const
147			{
148	✓✗	46	data.U = I.col(Inertia::ANGULAR + axis);
149		46	data.Dinv[0] = (Scalar)(1)/I(Inertia::ANGULAR + axis,Inertia::ANGULAR + axis);
150	✓✗✓✗	46	data.UDinv.noalias() = data.U * data.Dinv[0];
151
152	✓✓	46	if (update_I)
153	✓✗✓✗	12	PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like,I) -= data.UDinv * data.U.transpose();
154		46	}
155
156		104445	static std::string classname()
157			{
158	✓✗✓✗	104445	return std::string("JointModelRUB") + axisLabel<axis>();
159			}
160		52098	std::string shortname() const { return classname(); }
161
162			/// \returns An expression of *this with the Scalar type casted to NewScalar.
163			template<typename NewScalar>
164		12	JointModelRevoluteUnboundedTpl<NewScalar,Options,axis> cast() const
165			{
166			typedef JointModelRevoluteUnboundedTpl<NewScalar,Options,axis> ReturnType;
167		12	ReturnType res;
168		12	res.setIndexes(id(),idx_q(),idx_v());
169		12	return res;
170			}
171
172			}; // struct JointModelRevoluteUnboundedTpl
173
174			struct UnboundedRevoluteAffineTransform
175			{
176			template<typename ConfigVectorIn, typename Scalar, typename ConfigVectorOut>
177		4	static void run(const Eigen::MatrixBase<ConfigVectorIn> & q,
178			const Scalar & scaling,
179			const Scalar & offset,
180			const Eigen::MatrixBase<ConfigVectorOut> & dest)
181			{
182			EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(ConfigVectorIn,2);
183			EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(ConfigVectorOut,2);
184
185		4	const typename ConfigVectorIn::Scalar & ca = q(0);
186		4	const typename ConfigVectorIn::Scalar & sa = q(1);
187
188		4	const typename ConfigVectorIn::Scalar & theta = math::atan2(sa,ca);
189		4	const typename ConfigVectorIn::Scalar & theta_transform = scaling * theta + offset;
190
191		4	ConfigVectorOut & dest_ = PINOCCHIO_EIGEN_CONST_CAST(ConfigVectorOut,dest);
192		4	SINCOS(theta_transform,&dest_.coeffRef(1),&dest_.coeffRef(0));
193		4	}
194			};
195
196			template<typename Scalar, int Options, int axis>
197			struct ConfigVectorAffineTransform< JointRevoluteUnboundedTpl<Scalar,Options,axis> >
198			{
199			typedef UnboundedRevoluteAffineTransform Type;
200			};
201
202			typedef JointRevoluteUnboundedTpl<double,0,0> JointRUBX;
203			typedef JointDataRevoluteUnboundedTpl<double,0,0> JointDataRUBX;
204			typedef JointModelRevoluteUnboundedTpl<double,0,0> JointModelRUBX;
205
206			typedef JointRevoluteUnboundedTpl<double,0,1> JointRUBY;
207			typedef JointDataRevoluteUnboundedTpl<double,0,1> JointDataRUBY;
208			typedef JointModelRevoluteUnboundedTpl<double,0,1> JointModelRUBY;
209
210			typedef JointRevoluteUnboundedTpl<double,0,2> JointRUBZ;
211			typedef JointDataRevoluteUnboundedTpl<double,0,2> JointDataRUBZ;
212			typedef JointModelRevoluteUnboundedTpl<double,0,2> JointModelRUBZ;
213
214			} //namespace pinocchio
215
216			#include <boost/type_traits.hpp>
217
218			namespace boost
219			{
220			template<typename Scalar, int Options, int axis>
221			struct has_nothrow_constructor< ::pinocchio::JointModelRevoluteUnboundedTpl<Scalar,Options,axis> >
222			: public integral_constant<bool,true> {};
223
224			template<typename Scalar, int Options, int axis>
225			struct has_nothrow_copy< ::pinocchio::JointModelRevoluteUnboundedTpl<Scalar,Options,axis> >
226			: public integral_constant<bool,true> {};
227
228			template<typename Scalar, int Options, int axis>
229			struct has_nothrow_constructor< ::pinocchio::JointDataRevoluteUnboundedTpl<Scalar,Options,axis> >
230			: public integral_constant<bool,true> {};
231
232			template<typename Scalar, int Options, int axis>
233			struct has_nothrow_copy< ::pinocchio::JointDataRevoluteUnboundedTpl<Scalar,Options,axis> >
234			: public integral_constant<bool,true> {};
235			}
236
237			#endif // ifndef __pinocchio_joint_revolute_unbounded_hpp__