GCC Code Coverage Report

Directory: ./
File: include/pinocchio/spatial/force-dense.hpp
Date: 2025-02-12 21:03:38
Exec Total Coverage
Lines: 90 92 97.8%
Branches: 119 249 47.8%
Line Branch Exec Source
1 //
2 // Copyright (c) 2017-2020 CNRS INRIA
3 //
4
5 #ifndef __pinocchio_spatial_force_dense_hpp__
6 #define __pinocchio_spatial_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::angular;
32 using Base::derived;
33 using Base::isApprox;
34 using Base::isZero;
35 using Base::linear;
36 using Base::operator=;
37
38 3221 Derived & setZero()
39 {
40
1/2
✓ Branch 2 taken 3215 times.
✗ Branch 3 not taken.
 3221 linear().setZero();
41
1/2
✓ Branch 2 taken 3215 times.
✗ Branch 3 not taken.
 3221 angular().setZero();
42 3221 return derived();
43 }
44 146 Derived & setRandom()
45 {
46
1/2
✓ Branch 2 taken 146 times.
✗ Branch 3 not taken.
 149 linear().setRandom();
47
1/2
✓ Branch 2 taken 146 times.
✗ Branch 3 not taken.
 149 angular().setRandom();
48 149 return derived();
49 }
50
51 template<typename D2>
52 1591 bool isEqual_impl(const ForceDense<D2> & other) const
53 {
54
9/16
✓ Branch 1 taken 1591 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1591 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1591 times.
✗ Branch 8 not taken.
✓ Branch 9 taken 1590 times.
✓ Branch 10 taken 1 times.
✓ Branch 12 taken 1590 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 1590 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1590 times.
✗ Branch 19 not taken.
✓ Branch 20 taken 1590 times.
✗ Branch 21 not taken.
 1591 return linear() == other.linear() && angular() == other.angular();
55 }
56
57 template<typename D2>
58 bool isEqual_impl(const ForceBase<D2> & other) const
59 {
60 return other.derived() == derived();
61 }
62
63 // Arithmetic operators
64 template<typename D2>
65 4945 Derived & operator=(const ForceDense<D2> & other)
66 {
67 4945 return derived().set(other.derived());
68 }
69
70 10 Derived & operator=(const ForceDense & other)
71 {
72 10 return derived().set(other.derived());
73 }
74
75 template<typename D2>
76 4955 Derived & set(const ForceDense<D2> & other)
77 {
78
2/4
✓ Branch 2 taken 4887 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 4887 times.
✗ Branch 6 not taken.
 4955 linear() = other.linear();
79
2/4
✓ Branch 2 taken 4887 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 4887 times.
✗ Branch 6 not taken.
 4955 angular() = other.angular();
80 4955 return derived();
81 }
82
83 template<typename V6>
84 2 Derived & operator=(const Eigen::MatrixBase<V6> & v)
85 {
86 EIGEN_STATIC_ASSERT_VECTOR_ONLY(V6);
87
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 2 times.
 2 assert(v.size() == 6);
88
2/4
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
 2 linear() = v.template segment<3>(LINEAR);
89
2/4
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
 2 angular() = v.template segment<3>(ANGULAR);
90 2 return derived();
91 }
92
93 1189 ForcePlain operator-() const
94 {
95 1189 return derived().__opposite__();
96 }
97 template<typename F1>
98 163057 ForcePlain operator+(const ForceDense<F1> & f) const
99 {
100 163057 return derived().__plus__(f.derived());
101 }
102 template<typename F1>
103 427 ForcePlain operator-(const ForceDense<F1> & f) const
104 {
105 427 return derived().__minus__(f.derived());
106 }
107
108 template<typename F1>
109 162815 Derived & operator+=(const ForceDense<F1> & f)
110 {
111 162815 return derived().__pequ__(f.derived());
112 }
113 template<typename F1>
114 Derived & operator+=(const ForceBase<F1> & f)
115 {
116 f.derived().addTo(derived());
117 return derived();
118 }
119
120 template<typename M1>
121 8104 Derived & operator-=(const ForceDense<M1> & v)
122 {
123 8104 return derived().__mequ__(v.derived());
124 }
125
126 1189 ForcePlain __opposite__() const
127 {
128
4/8
✓ Branch 2 taken 1189 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1189 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1189 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1189 times.
✗ Branch 12 not taken.
 1189 return ForcePlain(-linear(), -angular());
129 }
130
131 template<typename M1>
132 163057 ForcePlain __plus__(const ForceDense<M1> & v) const
133 {
134
6/12
✓ Branch 2 taken 162947 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 162947 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 162947 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 162947 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 162947 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 162947 times.
✗ Branch 18 not taken.
 163057 return ForcePlain(linear() + v.linear(), angular() + v.angular());
135 }
136
137 template<typename M1>
138 427 ForcePlain __minus__(const ForceDense<M1> & v) const
139 {
140
6/12
✓ Branch 2 taken 427 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 427 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 427 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 427 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 427 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 427 times.
✗ Branch 18 not taken.
 427 return ForcePlain(linear() - v.linear(), angular() - v.angular());
141 }
142
143 template<typename M1>
144 162815 Derived & __pequ__(const ForceDense<M1> & v)
145 {
146
2/4
✓ Branch 2 taken 132640 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 132640 times.
✗ Branch 6 not taken.
 162815 linear() += v.linear();
147
2/4
✓ Branch 2 taken 132640 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 132640 times.
✗ Branch 6 not taken.
 162815 angular() += v.angular();
148 162815 return derived();
149 }
150
151 template<typename M1>
152 8104 Derived & __mequ__(const ForceDense<M1> & v)
153 {
154
2/4
✓ Branch 2 taken 8104 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 8104 times.
✗ Branch 6 not taken.
 8104 linear() -= v.linear();
155
2/4
✓ Branch 2 taken 8104 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 8104 times.
✗ Branch 6 not taken.
 8104 angular() -= v.angular();
156 8104 return derived();
157 }
158
159 template<typename OtherScalar>
160 154 ForcePlain __mult__(const OtherScalar & alpha) const
161 {
162
4/8
✓ Branch 2 taken 150 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 150 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 150 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 150 times.
✗ Branch 12 not taken.
 154 return ForcePlain(alpha * linear(), alpha * angular());
163 }
164
165 template<typename OtherScalar>
166 1 ForcePlain __div__(const OtherScalar & alpha) const
167 {
168
1/5
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
 2 return derived().__mult__((OtherScalar)(1) / alpha);
169 }
170
171 template<typename F1>
172 ✗ Scalar dot(const MotionDense<F1> & phi) const
173 {
174 ✗ return phi.linear().dot(linear()) + phi.angular().dot(angular());
175 }
176
177 template<typename M1, typename M2>
178 100866 void motionAction(const MotionDense<M1> & v, ForceDense<M2> & fout) const
179 {
180
5/10
✓ Branch 2 taken 90637 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 90637 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 90637 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 90637 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 90637 times.
✗ Branch 15 not taken.
 100866 fout.linear().noalias() = v.angular().cross(linear());
181
9/18
✓ Branch 2 taken 90637 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 90637 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 90637 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 90637 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 90637 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 90637 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 90637 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 90637 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 90637 times.
✗ Branch 27 not taken.
 100866 fout.angular().noalias() = v.angular().cross(angular()) + v.linear().cross(linear());
182 100866 }
183
184 template<typename M1>
185 100332 ForcePlain motionAction(const MotionDense<M1> & v) const
186 {
187 100332 ForcePlain res;
188
1/2
✓ Branch 1 taken 601 times.
✗ Branch 2 not taken.
 100332 motionAction(v, res);
189 100332 return res;
190 }
191
192 template<typename M2>
193 30 bool isApprox(
194 const ForceDense<M2> & f,
195 const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const
196 {
197 30 return derived().isApprox_impl(f, prec);
198 }
199
200 template<typename D2>
201 2517 bool isApprox_impl(
202 const ForceDense<D2> & f,
203 const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const
204 {
205
9/16
✓ Branch 1 taken 2477 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2477 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2477 times.
✗ Branch 8 not taken.
✓ Branch 9 taken 2475 times.
✓ Branch 10 taken 2 times.
✓ Branch 12 taken 2475 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 2475 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 2475 times.
✗ Branch 19 not taken.
✓ Branch 20 taken 2475 times.
✗ Branch 21 not taken.
 2517 return linear().isApprox(f.linear(), prec) && angular().isApprox(f.angular(), prec);
206 }
207
208 2 bool isZero_impl(const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const
209 {
210
7/12
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 1 times.
✓ Branch 7 taken 1 times.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 1 times.
✗ Branch 13 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
 2 return linear().isZero(prec) && angular().isZero(prec);
211 }
212
213 template<typename S2, int O2, typename D2>
214 51041 void se3Action_impl(const SE3Tpl<S2, O2> & m, ForceDense<D2> & f) const
215 {
216
5/10
✓ Branch 2 taken 49442 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 49442 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 49442 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 49442 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 49442 times.
✗ Branch 15 not taken.
 51041 f.linear().noalias() = m.rotation() * linear();
217
5/10
✓ Branch 2 taken 49442 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 49442 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 49442 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 49442 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 49442 times.
✗ Branch 15 not taken.
 51041 f.angular().noalias() = m.rotation() * angular();
218
3/6
✓ Branch 3 taken 49442 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 49442 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 49442 times.
✗ Branch 10 not taken.
 51041 f.angular() += m.translation().cross(f.linear());
219 51041 }
220
221 template<typename S2, int O2>
222 51041 ForcePlain se3Action_impl(const SE3Tpl<S2, O2> & m) const
223 {
224 51041 ForcePlain res;
225
1/2
✓ Branch 1 taken 509 times.
✗ Branch 2 not taken.
 51041 se3Action_impl(m, res);
226 51041 return res;
227 }
228
229 template<typename S2, int O2, typename D2>
230 187 void se3ActionInverse_impl(const SE3Tpl<S2, O2> & m, ForceDense<D2> & f) const
231 {
232
6/12
✓ Branch 2 taken 104 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 104 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 104 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 104 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 104 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 104 times.
✗ Branch 18 not taken.
 187 f.linear().noalias() = m.rotation().transpose() * linear();
233
6/12
✓ Branch 1 taken 104 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 104 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 104 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 104 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 104 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 104 times.
✗ Branch 17 not taken.
 561 f.angular().noalias() =
234
3/6
✓ Branch 3 taken 104 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 104 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 104 times.
✗ Branch 10 not taken.
 561 m.rotation().transpose() * (angular() - m.translation().cross(linear()));
235 187 }
236
237 template<typename S2, int O2>
238 187 ForcePlain se3ActionInverse_impl(const SE3Tpl<S2, O2> & m) const
239 {
240 187 ForcePlain res;
241
0/2
✗ Branch 1 not taken.
✗ Branch 2 not taken.
 187 se3ActionInverse_impl(m, res);
242 187 return res;
243 }
244
245 9 void disp_impl(std::ostream & os) const
246 {
247
3/6
✓ Branch 3 taken 9 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 9 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 9 times.
✗ Branch 10 not taken.
 9 os << " f = " << linear().transpose() << std::endl
248
5/10
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 9 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 9 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 9 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 9 times.
✗ Branch 14 not taken.
 9 << "tau = " << angular().transpose() << std::endl;
249 9 }
250
251 /// \returns a ForceRef on this.
252 ForceRefType ref()
253 {
254 return derived().ref();
255 }
256
257 protected:
258 1353737 ForceDense() {};
259
260 ForceDense(const ForceDense &) = delete;
261
262 }; // class ForceDense
263
264 /// Basic operations specialization
265 template<typename F1>
266 typename traits<F1>::ForcePlain
267 146 operator*(const typename traits<F1>::Scalar alpha, const ForceDense<F1> & f)
268 {
269 146 return f.derived() * alpha;
270 }
271
272 } // namespace pinocchio
273
274 #endif // ifndef __pinocchio_spatial_force_dense_hpp__
275