Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	include/pinocchio/spatial/motion-dense.hpp	Lines:	84	92	91.3 %
Date:	2024-04-26 13:14:21	Branches:	119	248	48.0 %


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

#ifndef __pinocchio_motion_dense_hpp__
#define __pinocchio_motion_dense_hpp__

#include "pinocchio/spatial/skew.hpp"

namespace pinocchio
{

  template<typename Derived>
  struct SE3GroupAction< MotionDense<Derived> >
  {
    typedef typename SE3GroupAction< Derived >::ReturnType ReturnType;
  };

  template<typename Derived, typename MotionDerived>
  struct MotionAlgebraAction< MotionDense<Derived>, MotionDerived >
  {
    typedef typename MotionAlgebraAction< Derived, MotionDerived >::ReturnType ReturnType;
  };

  template<typename Derived>
  class MotionDense : public MotionBase<Derived>
  {
  public:
    typedef MotionBase<Derived> Base;
    MOTION_TYPEDEF_TPL(Derived);
    typedef typename traits<Derived>::MotionRefType MotionRefType;

    using Base::linear;
    using Base::angular;
    using Base::derived;
    using Base::isApprox;
    using Base::isZero;

    Derived & setZero() { linear().setZero(); angular().setZero(); return derived(); }
    Derived & setRandom() { linear().setRandom(); angular().setRandom(); return derived(); }

    ActionMatrixType toActionMatrix_impl() const
    {
      ActionMatrixType X;
      X.template block <3,3> (ANGULAR, ANGULAR) = X.template block <3,3> (LINEAR, LINEAR) = skew(angular());
      X.template block <3,3> (LINEAR, ANGULAR) = skew(linear());
      X.template block <3,3> (ANGULAR, LINEAR).setZero();

      return X;
    }

    ActionMatrixType toDualActionMatrix_impl() const
    {
      ActionMatrixType X;
      X.template block <3,3> (ANGULAR, ANGULAR) = X.template block <3,3> (LINEAR, LINEAR) = skew(angular());
      X.template block <3,3> (ANGULAR, LINEAR) = skew(linear());
      X.template block <3,3> (LINEAR, ANGULAR).setZero();

      return X;
    }

    HomogeneousMatrixType toHomogeneousMatrix_impl() const
    {
      HomogeneousMatrixType M;
      M.template block<3,3>(0, 0) = skew(angular());
      M.template block<3,1>(0, 3) = linear();
      M.template block<1,4>(3, 0).setZero();
      return M;
    }

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

    template<typename D2>
    bool isEqual_impl(const MotionBase<D2> & other) const
    { return other.derived() == derived(); }

    // Arithmetic operators
    template<typename D2>
    Derived & operator=(const MotionDense<D2> & other)
    {
      linear() = other.linear();
      angular() = other.angular();
      return derived();
    }

    template<typename D2>
    Derived & operator=(const MotionBase<D2> & other)
    {
      other.derived().setTo(derived());
      return derived();
    }

    template<typename V6>
    Derived & operator=(const Eigen::MatrixBase<V6> & v)
    {
      EIGEN_STATIC_ASSERT_VECTOR_ONLY(V6); assert(v.size() == 6);
      linear() = v.template segment<3>(LINEAR);
      angular() = v.template segment<3>(ANGULAR);
      return derived();
    }

    MotionPlain operator-() const { return derived().__opposite__(); }
    template<typename M1>
    MotionPlain operator+(const MotionDense<M1> & v) const { return derived().__plus__(v.derived()); }
    template<typename M1>
    MotionPlain operator-(const MotionDense<M1> & v) const { return derived().__minus__(v.derived()); }

    template<typename M1>
    Derived & operator+=(const MotionDense<M1> & v) { return derived().__pequ__(v.derived()); }
    template<typename M1>
    Derived & operator+=(const MotionBase<M1> & v)
    { v.derived().addTo(derived()); return derived(); }

    template<typename M1>
    Derived & operator-=(const MotionDense<M1> & v) { return derived().__mequ__(v.derived()); }

    MotionPlain __opposite__() const { return MotionPlain(-linear(),-angular()); }

    template<typename M1>
    MotionPlain __plus__(const MotionDense<M1> & v) const
    { return MotionPlain(linear()+v.linear(), angular()+v.angular()); }

    template<typename M1>
    MotionPlain __minus__(const MotionDense<M1> & v) const
    { return MotionPlain(linear()-v.linear(), angular()-v.angular()); }

    template<typename M1>
    Derived & __pequ__(const MotionDense<M1> & v)
    { linear() += v.linear(); angular() += v.angular(); return derived(); }

    template<typename M1>
    Derived & __mequ__(const MotionDense<M1> & v)
    { linear() -= v.linear(); angular() -= v.angular(); return derived(); }

    template<typename OtherScalar>
    MotionPlain __mult__(const OtherScalar & alpha) const
    { return MotionPlain(alpha*linear(),alpha*angular()); }

    template<typename OtherScalar>
    MotionPlain __div__(const OtherScalar & alpha) const
    { return derived().__mult__((OtherScalar)(1)/alpha); }

    template<typename F1>
    Scalar dot(const ForceBase<F1> & phi) const
    { return phi.linear().dot(linear()) + phi.angular().dot(angular()); }

    template<typename D>
    typename MotionAlgebraAction<D,Derived>::ReturnType cross_impl(const D & d) const
    {
      return d.motionAction(derived());
    }

    template<typename M1, typename M2>
    void motionAction(const MotionDense<M1> & v, MotionDense<M2> & mout) const
    {
      mout.linear() = v.linear().cross(angular())+v.angular().cross(linear());
      mout.angular() = v.angular().cross(angular());
    }

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

    template<typename M2>
    bool isApprox(const MotionDense<M2> & m2, const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const
    {
      return derived().isApprox_impl(m2, prec);
    }

    template<typename D2>
    bool isApprox_impl(const MotionDense<D2> & m2, const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const
    {
      return linear().isApprox(m2.linear(), prec) && angular().isApprox(m2.angular(), prec);
    }

    bool isZero_impl(const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const
    {
      return linear().isZero(prec) && angular().isZero(prec);
    }

    template<typename S2, int O2, typename D2>
    void se3Action_impl(const SE3Tpl<S2,O2> & m, MotionDense<D2> & v) const
    {
      v.angular().noalias() = m.rotation()*angular();
      v.linear().noalias() = m.rotation()*linear() + m.translation().cross(v.angular());
    }

    template<typename S2, int O2>
    typename SE3GroupAction<Derived>::ReturnType
    se3Action_impl(const SE3Tpl<S2,O2> & m) const
    {
      typename SE3GroupAction<Derived>::ReturnType 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
    {
      v.linear().noalias() = m.rotation().transpose()*(linear()-m.translation().cross(angular()));
      v.angular().noalias() = m.rotation().transpose()*angular();
    }

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

    void disp_impl(std::ostream & os) const
    {
      os
      << "  v = " << linear().transpose () << std::endl
      << "  w = " << angular().transpose () << std::endl;
    }

    /// \returns a MotionRef on this.
    MotionRefType ref() { return derived().ref(); }

  }; // class MotionDense

  /// Basic operations specialization
  template<typename M1, typename M2>
  typename traits<M1>::MotionPlain operator^(const MotionDense<M1> & v1,
                                             const MotionDense<M2> & v2)
  { return v1.derived().cross(v2.derived()); }

  template<typename M1, typename F1>
  typename traits<F1>::ForcePlain operator^(const MotionDense<M1> & v,
                                            const ForceBase<F1> & f)
  { return v.derived().cross(f.derived()); }

  template<typename M1>
  typename traits<M1>::MotionPlain operator*(const typename traits<M1>::Scalar alpha,
                                             const MotionDense<M1> & v)
  { return v*alpha; }

} // namespace pinocchio

#endif // ifndef __pinocchio_motion_dense_hpp__


Generated by: GCOVR (Version 4.2)

Line	Branch	Exec	Source
1			//
2			// Copyright (c) 2017-2019 CNRS INRIA
3			//
4
5			#ifndef __pinocchio_motion_dense_hpp__
6			#define __pinocchio_motion_dense_hpp__
7
8			#include "pinocchio/spatial/skew.hpp"
9
10			namespace pinocchio
11			{
12
13			template<typename Derived>
14			struct SE3GroupAction< MotionDense<Derived> >
15			{
16			typedef typename SE3GroupAction< Derived >::ReturnType ReturnType;
17			};
18
19			template<typename Derived, typename MotionDerived>
20			struct MotionAlgebraAction< MotionDense<Derived>, MotionDerived >
21			{
22			typedef typename MotionAlgebraAction< Derived, MotionDerived >::ReturnType ReturnType;
23			};
24
25			template<typename Derived>
26			class MotionDense : public MotionBase<Derived>
27			{
28			public:
29			typedef MotionBase<Derived> Base;
30			MOTION_TYPEDEF_TPL(Derived);
31			typedef typename traits<Derived>::MotionRefType MotionRefType;
32
33			using Base::linear;
34			using Base::angular;
35			using Base::derived;
36			using Base::isApprox;
37			using Base::isZero;
38
39	✓✗✓✗	5871	Derived & setZero() { linear().setZero(); angular().setZero(); return derived(); }
40	✓✗✓✗	2	Derived & setRandom() { linear().setRandom(); angular().setRandom(); return derived(); }
41
42		158	ActionMatrixType toActionMatrix_impl() const
43			{
44		158	ActionMatrixType X;
45	✓✗✓✗ ✓✗✓✗ ✓✗	158	X.template block <3,3> (ANGULAR, ANGULAR) = X.template block <3,3> (LINEAR, LINEAR) = skew(angular());
46	✓✗✓✗ ✓✗	158	X.template block <3,3> (LINEAR, ANGULAR) = skew(linear());
47	✓✗	158	X.template block <3,3> (ANGULAR, LINEAR).setZero();
48
49		158	return X;
50			}
51
52		6	ActionMatrixType toDualActionMatrix_impl() const
53			{
54		6	ActionMatrixType X;
55	✓✗✓✗ ✓✗✓✗ ✓✗	6	X.template block <3,3> (ANGULAR, ANGULAR) = X.template block <3,3> (LINEAR, LINEAR) = skew(angular());
56	✓✗✓✗ ✓✗	6	X.template block <3,3> (ANGULAR, LINEAR) = skew(linear());
57	✓✗	6	X.template block <3,3> (LINEAR, ANGULAR).setZero();
58
59		6	return X;
60			}
61
62			HomogeneousMatrixType toHomogeneousMatrix_impl() const
63			{
64			HomogeneousMatrixType M;
65			M.template block<3,3>(0, 0) = skew(angular());
66			M.template block<3,1>(0, 3) = linear();
67			M.template block<1,4>(3, 0).setZero();
68			return M;
69			}
70
71			template<typename D2>
72		2592	bool isEqual_impl(const MotionDense<D2> & other) const
73	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗	2592	{ return linear() == other.linear() && angular() == other.angular(); }
74
75			template<typename D2>
76		40	bool isEqual_impl(const MotionBase<D2> & other) const
77		40	{ return other.derived() == derived(); }
78
79			// Arithmetic operators
80			template<typename D2>
81		10580	Derived & operator=(const MotionDense<D2> & other)
82			{
83	✓✗✓✗	10580	linear() = other.linear();
84	✓✗✓✗	10580	angular() = other.angular();
85		10580	return derived();
86			}
87
88			template<typename D2>
89		85982	Derived & operator=(const MotionBase<D2> & other)
90			{
91		85982	other.derived().setTo(derived());
92		85982	return derived();
93			}
94
95			template<typename V6>
96		4032	Derived & operator=(const Eigen::MatrixBase<V6> & v)
97			{
98	✗✓	4032	EIGEN_STATIC_ASSERT_VECTOR_ONLY(V6); assert(v.size() == 6);
99	✓✗✓✗	4032	linear() = v.template segment<3>(LINEAR);
100	✓✗✓✗	4032	angular() = v.template segment<3>(ANGULAR);
101		4032	return derived();
102			}
103
104		1608	MotionPlain operator-() const { return derived().__opposite__(); }
105			template<typename M1>
106		3255	MotionPlain operator+(const MotionDense<M1> & v) const { return derived().__plus__(v.derived()); }
107			template<typename M1>
108		5773	MotionPlain operator-(const MotionDense<M1> & v) const { return derived().__minus__(v.derived()); }
109
110			template<typename M1>
111		182898	Derived & operator+=(const MotionDense<M1> & v) { return derived().__pequ__(v.derived()); }
112			template<typename M1>
113		62824	Derived & operator+=(const MotionBase<M1> & v)
114		62824	{ v.derived().addTo(derived()); return derived(); }
115
116			template<typename M1>
117		147	Derived & operator-=(const MotionDense<M1> & v) { return derived().__mequ__(v.derived()); }
118
119	✓✗✓✗ ✓✗✓✗	1608	MotionPlain __opposite__() const { return MotionPlain(-linear(),-angular()); }
120
121			template<typename M1>
122		128	MotionPlain __plus__(const MotionDense<M1> & v) const
123	✓✗✓✗ ✓✗✓✗ ✓✗✓✗	128	{ return MotionPlain(linear()+v.linear(), angular()+v.angular()); }
124
125			template<typename M1>
126		1	MotionPlain __minus__(const MotionDense<M1> & v) const
127	✓✗✓✗ ✓✗✓✗ ✓✗✓✗	1	{ return MotionPlain(linear()-v.linear(), angular()-v.angular()); }
128
129			template<typename M1>
130			Derived & __pequ__(const MotionDense<M1> & v)
131			{ linear() += v.linear(); angular() += v.angular(); return derived(); }
132
133			template<typename M1>
134			Derived & __mequ__(const MotionDense<M1> & v)
135			{ linear() -= v.linear(); angular() -= v.angular(); return derived(); }
136
137			template<typename OtherScalar>
138			MotionPlain __mult__(const OtherScalar & alpha) const
139			{ return MotionPlain(alphalinear(),alphaangular()); }
140
141			template<typename OtherScalar>
142		1	MotionPlain __div__(const OtherScalar & alpha) const
143	✓✗	2	{ return derived().__mult__((OtherScalar)(1)/alpha); }
144
145			template<typename F1>
146			Scalar dot(const ForceBase<F1> & phi) const
147			{ return phi.linear().dot(linear()) + phi.angular().dot(angular()); }
148
149			template<typename D>
150		78692	typename MotionAlgebraAction<D,Derived>::ReturnType cross_impl(const D & d) const
151			{
152		78692	return d.motionAction(derived());
153			}
154
155			template<typename M1, typename M2>
156		70192	void motionAction(const MotionDense<M1> & v, MotionDense<M2> & mout) const
157			{
158	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗	70192	mout.linear() = v.linear().cross(angular())+v.angular().cross(linear());
159	✓✗✓✗ ✓✗✓✗	70192	mout.angular() = v.angular().cross(angular());
160		70192	}
161
162			template<typename M1>
163		15346	MotionPlain motionAction(const MotionDense<M1> & v) const
164			{
165		15346	MotionPlain res;
166		15346	motionAction(v,res);
167		15346	return res;
168			}
169
170			template<typename M2>
171		16	bool isApprox(const MotionDense<M2> & m2, const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const
172			{
173		16	return derived().isApprox_impl(m2, prec);
174			}
175
176			template<typename D2>
177		1247	bool isApprox_impl(const MotionDense<D2> & m2, const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const
178			{
179	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗	1247	return linear().isApprox(m2.linear(), prec) && angular().isApprox(m2.angular(), prec);
180			}
181
182		10	bool isZero_impl(const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const
183			{
184	✓✗✓✗ ✓✓✓✗ ✓✗✓✗	10	return linear().isZero(prec) && angular().isZero(prec);
185			}
186
187			template<typename S2, int O2, typename D2>
188		64977	void se3Action_impl(const SE3Tpl<S2,O2> & m, MotionDense<D2> & v) const
189			{
190	✓✗✓✗ ✓✗✓✗ ✓✗	64977	v.angular().noalias() = m.rotation()*angular();
191	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗	64977	v.linear().noalias() = m.rotation()*linear() + m.translation().cross(v.angular());
192		64977	}
193
194			template<typename S2, int O2>
195			typename SE3GroupAction<Derived>::ReturnType
196		64977	se3Action_impl(const SE3Tpl<S2,O2> & m) const
197			{
198		64977	typename SE3GroupAction<Derived>::ReturnType res;
199		64977	se3Action_impl(m,res);
200		64977	return res;
201			}
202
203			template<typename S2, int O2, typename D2>
204		184199	void se3ActionInverse_impl(const SE3Tpl<S2,O2> & m, MotionDense<D2> & v) const
205			{
206	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	184199	v.linear().noalias() = m.rotation().transpose()*(linear()-m.translation().cross(angular()));
207	✓✗✓✗ ✓✗✓✗ ✓✗✓✗	184199	v.angular().noalias() = m.rotation().transpose()*angular();
208		184199	}
209
210			template<typename S2, int O2>
211			typename SE3GroupAction<Derived>::ReturnType
212		184114	se3ActionInverse_impl(const SE3Tpl<S2,O2> & m) const
213			{
214		184114	typename SE3GroupAction<Derived>::ReturnType res;
215		184114	se3ActionInverse_impl(m,res);
216		184114	return res;
217			}
218
219		1	void disp_impl(std::ostream & os) const
220			{
221			os
222	✓✗✓✗ ✓✗	1	<< " v = " << linear().transpose () << std::endl
223	✓✗✓✗ ✓✗✓✗ ✓✗	1	<< " w = " << angular().transpose () << std::endl;
224		1	}
225
226			/// \returns a MotionRef on this.
227			MotionRefType ref() { return derived().ref(); }
228
229			}; // class MotionDense
230
231			/// Basic operations specialization
232			template<typename M1, typename M2>
233		1538	typename traits<M1>::MotionPlain operator^(const MotionDense<M1> & v1,
234			const MotionDense<M2> & v2)
235		1538	{ return v1.derived().cross(v2.derived()); }
236
237			template<typename M1, typename F1>
238			typename traits<F1>::ForcePlain operator^(const MotionDense<M1> & v,
239			const ForceBase<F1> & f)
240			{ return v.derived().cross(f.derived()); }
241
242			template<typename M1>
243		13446	typename traits<M1>::MotionPlain operator*(const typename traits<M1>::Scalar alpha,
244			const MotionDense<M1> & v)
245		13446	{ return v*alpha; }
246
247			} // namespace pinocchio
248
249			#endif // ifndef __pinocchio_motion_dense_hpp__