Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	include/pinocchio/spatial/force-dense.hpp	Lines:	70	70	100.0 %
Date:	2024-01-23 21:41:47	Branches:	117	228	51.3 %


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

#ifndef __pinocchio_force_dense_hpp__
#define __pinocchio_force_dense_hpp__

namespace pinocchio
{

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

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

  template<typename Derived>
  class ForceDense : public ForceBase<Derived>
  {
  public:
    typedef ForceBase<Derived> Base;
    FORCE_TYPEDEF_TPL(Derived);
    typedef typename traits<Derived>::ForceRefType ForceRefType;

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

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

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

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

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

    template<typename D2>
    Derived & operator=(const ForceDense<D2> & other)
    {
      return derived().setFrom(other.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();
    }

    ForcePlain operator-() const { return derived().__opposite__(); }
    template<typename F1>
    ForcePlain operator+(const ForceDense<F1> & f) const { return derived().__plus__(f.derived()); }
    template<typename F1>
    ForcePlain operator-(const ForceDense<F1> & f) const { return derived().__minus__(f.derived()); }

    template<typename F1>
    Derived & operator+=(const ForceDense<F1> & f) { return derived().__pequ__(f.derived()); }
    template<typename F1>
    Derived & operator+=(const ForceBase<F1> & f)
    { f.derived().addTo(derived()); return derived(); }

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

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

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

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

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

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

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

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

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

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

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

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

    template<typename D2>
    bool isApprox_impl(const ForceDense<D2> & f, const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const
    {
      return linear().isApprox(f.linear(), prec) && angular().isApprox(f.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, ForceDense<D2> & f) const
    {
      f.linear().noalias() = m.rotation()*linear();
      f.angular().noalias() = m.rotation()*angular();
      f.angular() += m.translation().cross(f.linear());
    }

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

    template<typename S2, int O2, typename D2>
    void se3ActionInverse_impl(const SE3Tpl<S2,O2> & m, ForceDense<D2> & f) const
    {
      f.linear().noalias() = m.rotation().transpose()*linear();
      f.angular().noalias() = m.rotation().transpose()*(angular()-m.translation().cross(linear()));
    }

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

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

    /// \returns a ForceRef on this.
    ForceRefType ref() { return derived().ref(); }

  }; // class ForceDense

  /// Basic operations specialization
  template<typename F1>
  typename traits<F1>::ForcePlain operator*(const typename traits<F1>::Scalar alpha,
                                            const ForceDense<F1> & f)
  { return f.derived()*alpha; }

} // namespace pinocchio

#endif // ifndef __pinocchio_force_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_force_dense_hpp__
6			#define __pinocchio_force_dense_hpp__
7
8			namespace pinocchio
9			{
10
11			template<typename Derived>
12			struct SE3GroupAction< ForceDense<Derived> >
13			{
14			typedef typename SE3GroupAction< Derived >::ReturnType ReturnType;
15			};
16
17			template<typename Derived, typename MotionDerived>
18			struct MotionAlgebraAction< ForceDense<Derived>, MotionDerived >
19			{
20			typedef typename MotionAlgebraAction< Derived, MotionDerived >::ReturnType ReturnType;
21			};
22
23			template<typename Derived>
24			class ForceDense : public ForceBase<Derived>
25			{
26			public:
27			typedef ForceBase<Derived> Base;
28			FORCE_TYPEDEF_TPL(Derived);
29			typedef typename traits<Derived>::ForceRefType ForceRefType;
30
31			using Base::linear;
32			using Base::angular;
33			using Base::derived;
34			using Base::isApprox;
35			using Base::isZero;
36			using Base::operator=;
37
38	✓✗✓✗	452	Derived & setZero() { linear().setZero(); angular().setZero(); return derived(); }
39	✓✗✓✗	116	Derived & setRandom() { linear().setRandom(); angular().setRandom(); return derived(); }
40
41			template<typename D2>
42		1150	bool isEqual_impl(const ForceDense<D2> & other) const
43	✓✗✓✗ ✓✗✓✓ ✓✗✓✗ ✓✗✓✗	1150	{ return linear() == other.linear() && angular() == other.angular(); }
44
45			template<typename D2>
46			bool isEqual_impl(const ForceBase<D2> & other) const
47			{ return other.derived() == derived(); }
48
49			// Arithmetic operators
50			template<typename D2>
51		13228	Derived & setFrom(const ForceDense<D2> & other)
52			{
53	✓✗✓✗	13228	linear() = other.linear();
54	✓✗✓✗	13228	angular() = other.angular();
55		13228	return derived();
56			}
57
58			template<typename D2>
59		13228	Derived & operator=(const ForceDense<D2> & other)
60			{
61		13228	return derived().setFrom(other.derived());
62			}
63
64			template<typename V6>
65		1	Derived & operator=(const Eigen::MatrixBase<V6> & v)
66			{
67	✗✓	1	EIGEN_STATIC_ASSERT_VECTOR_ONLY(V6); assert(v.size() == 6);
68	✓✗✓✗	1	linear() = v.template segment<3>(LINEAR);
69	✓✗✓✗	1	angular() = v.template segment<3>(ANGULAR);
70		1	return derived();
71			}
72
73		1	ForcePlain operator-() const { return derived().__opposite__(); }
74			template<typename F1>
75		14416	ForcePlain operator+(const ForceDense<F1> & f) const { return derived().__plus__(f.derived()); }
76			template<typename F1>
77		204	ForcePlain operator-(const ForceDense<F1> & f) const { return derived().__minus__(f.derived()); }
78
79			template<typename F1>
80		145352	Derived & operator+=(const ForceDense<F1> & f) { return derived().__pequ__(f.derived()); }
81			template<typename F1>
82			Derived & operator+=(const ForceBase<F1> & f)
83			{ f.derived().addTo(derived()); return derived(); }
84
85			template<typename M1>
86		6701	Derived & operator-=(const ForceDense<M1> & v) { return derived().__mequ__(v.derived()); }
87
88	✓✗✓✗ ✓✗✓✗	1	ForcePlain __opposite__() const { return ForcePlain(-linear(),-angular()); }
89
90			template<typename M1>
91		14416	ForcePlain __plus__(const ForceDense<M1> & v) const
92	✓✗✓✗ ✓✗✓✗ ✓✗✓✗	14416	{ return ForcePlain(linear()+v.linear(), angular()+v.angular()); }
93
94			template<typename M1>
95		204	ForcePlain __minus__(const ForceDense<M1> & v) const
96	✓✗✓✗ ✓✗✓✗ ✓✗✓✗	204	{ return ForcePlain(linear()-v.linear(), angular()-v.angular()); }
97
98			template<typename M1>
99		145352	Derived & __pequ__(const ForceDense<M1> & v)
100	✓✗✓✗ ✓✗✓✗	145352	{ linear() += v.linear(); angular() += v.angular(); return derived(); }
101
102			template<typename M1>
103		6701	Derived & __mequ__(const ForceDense<M1> & v)
104	✓✗✓✗ ✓✗✓✗	6701	{ linear() -= v.linear(); angular() -= v.angular(); return derived(); }
105
106			template<typename OtherScalar>
107		117	ForcePlain __mult__(const OtherScalar & alpha) const
108	✓✗✓✗ ✓✗✓✗	117	{ return ForcePlain(alphalinear(),alphaangular()); }
109
110			template<typename OtherScalar>
111		1	ForcePlain __div__(const OtherScalar & alpha) const
112	✓✗	2	{ return derived().__mult__((OtherScalar)(1)/alpha); }
113
114			template<typename F1>
115			Scalar dot(const MotionDense<F1> & phi) const
116			{ return phi.linear().dot(linear()) + phi.angular().dot(angular()); }
117
118			template<typename M1, typename M2>
119		60950	void motionAction(const MotionDense<M1> & v, ForceDense<M2> & fout) const
120			{
121	✓✗✓✗ ✓✗✓✗ ✓✗	60950	fout.linear().noalias() = v.angular().cross(linear());
122	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	60950	fout.angular().noalias() = v.angular().cross(angular())+v.linear().cross(linear());
123		60950	}
124
125			template<typename M1>
126		52803	ForcePlain motionAction(const MotionDense<M1> & v) const
127			{
128		52803	ForcePlain res;
129		52803	motionAction(v,res);
130		52803	return res;
131			}
132
133			template<typename M2>
134		16	bool isApprox(const ForceDense<M2> & f, const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const
135		16	{ return derived().isApprox_impl(f, prec);}
136
137			template<typename D2>
138		234	bool isApprox_impl(const ForceDense<D2> & f, const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const
139			{
140	✓✗✓✗ ✓✗✓✓ ✓✗✓✗ ✓✗✓✗	234	return linear().isApprox(f.linear(), prec) && angular().isApprox(f.angular(), prec);
141			}
142
143		2	bool isZero_impl(const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const
144			{
145	✓✗✓✗ ✓✓✓✗ ✓✗✓✗	2	return linear().isZero(prec) && angular().isZero(prec);
146			}
147
148			template<typename S2, int O2, typename D2>
149		69611	void se3Action_impl(const SE3Tpl<S2,O2> & m, ForceDense<D2> & f) const
150			{
151	✓✗✓✗ ✓✗✓✗ ✓✗	69611	f.linear().noalias() = m.rotation()*linear();
152	✓✗✓✗ ✓✗✓✗ ✓✗	69611	f.angular().noalias() = m.rotation()*angular();
153	✓✗✓✗ ✓✗	69611	f.angular() += m.translation().cross(f.linear());
154		69611	}
155
156			template<typename S2, int O2>
157		63455	ForcePlain se3Action_impl(const SE3Tpl<S2,O2> & m) const
158			{
159		63455	ForcePlain res;
160		63455	se3Action_impl(m,res);
161		63455	return res;
162			}
163
164			template<typename S2, int O2, typename D2>
165		965	void se3ActionInverse_impl(const SE3Tpl<S2,O2> & m, ForceDense<D2> & f) const
166			{
167	✓✗✓✗ ✓✗✓✗ ✓✗✓✗	965	f.linear().noalias() = m.rotation().transpose()*linear();
168	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	965	f.angular().noalias() = m.rotation().transpose()*(angular()-m.translation().cross(linear()));
169		965	}
170
171			template<typename S2, int O2>
172		965	ForcePlain se3ActionInverse_impl(const SE3Tpl<S2,O2> & m) const
173			{
174		965	ForcePlain res;
175		965	se3ActionInverse_impl(m,res);
176		965	return res;
177			}
178
179		1	void disp_impl(std::ostream & os) const
180			{
181			os
182	✓✗✓✗ ✓✗	1	<< " f = " << linear().transpose () << std::endl
183	✓✗✓✗ ✓✗✓✗ ✓✗	1	<< "tau = " << angular().transpose () << std::endl;
184		1	}
185
186			/// \returns a ForceRef on this.
187			ForceRefType ref() { return derived().ref(); }
188
189			}; // class ForceDense
190
191			/// Basic operations specialization
192			template<typename F1>
193		109	typename traits<F1>::ForcePlain operator*(const typename traits<F1>::Scalar alpha,
194			const ForceDense<F1> & f)
195		109	{ return f.derived()*alpha; }
196
197			} // namespace pinocchio
198
199			#endif // ifndef __pinocchio_force_dense_hpp__