GCC Code Coverage Report

Directory: ./
File: include/pinocchio/spatial/motion-dense.hpp
Date: 2024-08-27 18:20:05
Exec Total Coverage
Lines: 84 115 73.0%
Branches: 97 260 37.3%
Line Branch Exec Source
1 //
2 // Copyright (c) 2017-2020 CNRS INRIA
3 //
4
5 #ifndef __pinocchio_spatial_motion_dense_hpp__
6 #define __pinocchio_spatial_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::angular;
34 using Base::derived;
35 using Base::isApprox;
36 using Base::isZero;
37 using Base::linear;
38
39 96051 Derived & setZero()
40 {
41
1/2
✓ Branch 2 taken 48045 times.
✗ Branch 3 not taken.
 96051 linear().setZero();
42
1/2
✓ Branch 2 taken 48045 times.
✗ Branch 3 not taken.
 96051 angular().setZero();
43 96051 return derived();
44 }
45 ✗ Derived & setRandom()
46 {
47 ✗ linear().setRandom();
48 ✗ angular().setRandom();
49 ✗ return derived();
50 }
51
52 2 ActionMatrixType toActionMatrix_impl() const
53 {
54 2 ActionMatrixType X;
55
5/10
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 2 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 2 times.
✗ Branch 14 not taken.
 2 X.template block<3, 3>(ANGULAR, ANGULAR) = X.template block<3, 3>(LINEAR, LINEAR) =
56 2 skew(angular());
57
3/6
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 2 times.
✗ Branch 9 not taken.
 2 X.template block<3, 3>(LINEAR, ANGULAR) = skew(linear());
58
1/2
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
 2 X.template block<3, 3>(ANGULAR, LINEAR).setZero();
59
60 2 return X;
61 }
62
63 ✗ ActionMatrixType toDualActionMatrix_impl() const
64 {
65 ✗ ActionMatrixType X;
66 ✗ X.template block<3, 3>(ANGULAR, ANGULAR) = X.template block<3, 3>(LINEAR, LINEAR) =
67 ✗ skew(angular());
68 ✗ X.template block<3, 3>(ANGULAR, LINEAR) = skew(linear());
69 ✗ X.template block<3, 3>(LINEAR, ANGULAR).setZero();
70
71 ✗ return X;
72 }
73
74 ✗ HomogeneousMatrixType toHomogeneousMatrix_impl() const
75 {
76 ✗ HomogeneousMatrixType M;
77 ✗ M.template block<3, 3>(0, 0) = skew(angular());
78 ✗ M.template block<3, 1>(0, 3) = linear();
79 ✗ M.template block<1, 4>(3, 0).setZero();
80 ✗ return M;
81 }
82
83 template<typename D2>
84 1 bool isEqual_impl(const MotionDense<D2> & other) const
85 {
86
8/16
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 1 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
 1 return linear() == other.linear() && angular() == other.angular();
87 }
88
89 template<typename D2>
90 ✗ bool isEqual_impl(const MotionBase<D2> & other) const
91 {
92 ✗ return other.derived() == derived();
93 }
94
95 // Arithmetic operators
96 template<typename D2>
97 14375 Derived & operator=(const MotionDense<D2> & other)
98 {
99 14375 return derived().set(other.derived());
100 }
101
102 ✗ Derived & operator=(const MotionDense & other)
103 {
104 ✗ return derived().set(other.derived());
105 }
106
107 template<typename D2>
108 14375 Derived & set(const MotionDense<D2> & other)
109 {
110
2/4
✓ Branch 2 taken 7457 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 7457 times.
✗ Branch 6 not taken.
 14375 linear() = other.linear();
111
2/4
✓ Branch 2 taken 7457 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 7457 times.
✗ Branch 6 not taken.
 14375 angular() = other.angular();
112 14375 return derived();
113 }
114
115 template<typename D2>
116 175942 Derived & operator=(const MotionBase<D2> & other)
117 {
118 175942 other.derived().setTo(derived());
119 175942 return derived();
120 }
121
122 template<typename V6>
123 4956 Derived & operator=(const Eigen::MatrixBase<V6> & v)
124 {
125 EIGEN_STATIC_ASSERT_VECTOR_ONLY(V6);
126
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 4848 times.
 4956 assert(v.size() == 6);
127
2/4
✓ Branch 2 taken 4848 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 4848 times.
✗ Branch 6 not taken.
 4956 linear() = v.template segment<3>(LINEAR);
128
2/4
✓ Branch 2 taken 4848 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 4848 times.
✗ Branch 6 not taken.
 4956 angular() = v.template segment<3>(ANGULAR);
129 4956 return derived();
130 }
131
132 2477 MotionPlain operator-() const
133 {
134 2477 return derived().__opposite__();
135 }
136 template<typename M1>
137 2995 MotionPlain operator+(const MotionDense<M1> & v) const
138 {
139 2995 return derived().__plus__(v.derived());
140 }
141 template<typename M1>
142 46322 MotionPlain operator-(const MotionDense<M1> & v) const
143 {
144 46322 return derived().__minus__(v.derived());
145 }
146
147 template<typename M1>
148 440123 Derived & operator+=(const MotionDense<M1> & v)
149 {
150 440123 return derived().__pequ__(v.derived());
151 }
152 template<typename M1>
153 79490 Derived & operator+=(const MotionBase<M1> & v)
154 {
155 79490 v.derived().addTo(derived());
156 79490 return derived();
157 }
158
159 template<typename M1>
160 323 Derived & operator-=(const MotionDense<M1> & v)
161 {
162 323 return derived().__mequ__(v.derived());
163 }
164
165 2477 MotionPlain __opposite__() const
166 {
167
4/8
✓ Branch 2 taken 2477 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2477 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 2477 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 2477 times.
✗ Branch 12 not taken.
 2477 return MotionPlain(-linear(), -angular());
168 }
169
170 template<typename M1>
171 ✗ MotionPlain __plus__(const MotionDense<M1> & v) const
172 {
173 ✗ return MotionPlain(linear() + v.linear(), angular() + v.angular());
174 }
175
176 template<typename M1>
177 ✗ MotionPlain __minus__(const MotionDense<M1> & v) const
178 {
179 ✗ return MotionPlain(linear() - v.linear(), angular() - v.angular());
180 }
181
182 template<typename M1>
183 Derived & __pequ__(const MotionDense<M1> & v)
184 {
185 linear() += v.linear();
186 angular() += v.angular();
187 return derived();
188 }
189
190 template<typename M1>
191 Derived & __mequ__(const MotionDense<M1> & v)
192 {
193 linear() -= v.linear();
194 angular() -= v.angular();
195 return derived();
196 }
197
198 template<typename OtherScalar>
199 MotionPlain __mult__(const OtherScalar & alpha) const
200 {
201 return MotionPlain(alpha * linear(), alpha * angular());
202 }
203
204 template<typename OtherScalar>
205 3 MotionPlain __div__(const OtherScalar & alpha) const
206 {
207
1/2
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
 6 return derived().__mult__((OtherScalar)(1) / alpha);
208 }
209
210 template<typename F1>
211 ✗ Scalar dot(const ForceBase<F1> & phi) const
212 {
213 ✗ return phi.linear().dot(linear()) + phi.angular().dot(angular());
214 }
215
216 template<typename D>
217 234724 typename MotionAlgebraAction<D, Derived>::ReturnType cross_impl(const D & d) const
218 {
219 234724 return d.motionAction(derived());
220 }
221
222 template<typename M1, typename M2>
223 134262 void motionAction(const MotionDense<M1> & v, MotionDense<M2> & mout) const
224 {
225
8/16
✓ Branch 2 taken 68297 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 68297 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 68297 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 68297 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 68297 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 68297 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 68297 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 68297 times.
✗ Branch 24 not taken.
 134262 mout.linear() = v.linear().cross(angular()) + v.angular().cross(linear());
226
4/8
✓ Branch 2 taken 68297 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 68297 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 68297 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 68297 times.
✗ Branch 12 not taken.
 134262 mout.angular() = v.angular().cross(angular());
227 134262 }
228
229 template<typename M1>
230 87236 MotionPlain motionAction(const MotionDense<M1> & v) const
231 {
232 87236 MotionPlain res;
233 87236 motionAction(v, res);
234 87236 return res;
235 }
236
237 template<typename M2>
238 ✗ bool isApprox(
239 const MotionDense<M2> & m2,
240 const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const
241 {
242 ✗ return derived().isApprox_impl(m2, prec);
243 }
244
245 template<typename D2>
246 371 bool isApprox_impl(
247 const MotionDense<D2> & m2,
248 const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const
249 {
250
8/16
✓ Branch 1 taken 371 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 371 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 371 times.
✗ Branch 8 not taken.
✓ Branch 9 taken 371 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 371 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 371 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 371 times.
✗ Branch 19 not taken.
✓ Branch 20 taken 371 times.
✗ Branch 21 not taken.
 371 return linear().isApprox(m2.linear(), prec) && angular().isApprox(m2.angular(), prec);
251 }
252
253 91 bool isZero_impl(const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const
254 {
255
7/12
✓ Branch 1 taken 91 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 91 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 39 times.
✓ Branch 7 taken 52 times.
✓ Branch 9 taken 39 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 39 times.
✗ Branch 13 not taken.
✓ Branch 14 taken 39 times.
✗ Branch 15 not taken.
 91 return linear().isZero(prec) && angular().isZero(prec);
256 }
257
258 template<typename S2, int O2, typename D2>
259 33322 void se3Action_impl(const SE3Tpl<S2, O2> & m, MotionDense<D2> & v) const
260 {
261
5/10
✓ Branch 2 taken 16722 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 16722 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 16722 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 16722 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 16722 times.
✗ Branch 15 not taken.
 33322 v.angular().noalias() = m.rotation() * angular();
262
8/16
✓ Branch 3 taken 16722 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 16722 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 16722 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 16722 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 16722 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 16722 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 16722 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 16722 times.
✗ Branch 25 not taken.
 33322 v.linear().noalias() = m.rotation() * linear() + m.translation().cross(v.angular());
263 33322 }
264
265 template<typename S2, int O2>
266 33322 typename SE3GroupAction<Derived>::ReturnType se3Action_impl(const SE3Tpl<S2, O2> & m) const
267 {
268 33322 typename SE3GroupAction<Derived>::ReturnType res;
269 33322 se3Action_impl(m, res);
270 33322 return res;
271 }
272
273 template<typename S2, int O2, typename D2>
274 233066 void se3ActionInverse_impl(const SE3Tpl<S2, O2> & m, MotionDense<D2> & v) const
275 {
276
7/14
✓ Branch 1 taken 116643 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 116643 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 116643 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 116643 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 116643 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 116643 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 30 times.
✗ Branch 20 not taken.
 699138 v.linear().noalias() =
277
3/6
✓ Branch 3 taken 116643 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 116643 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 116613 times.
✗ Branch 10 not taken.
 699138 m.rotation().transpose() * (linear() - m.translation().cross(angular()));
278
6/12
✓ Branch 2 taken 116643 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 116643 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 116643 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 116643 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 116643 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 116643 times.
✗ Branch 18 not taken.
 233066 v.angular().noalias() = m.rotation().transpose() * angular();
279 233066 }
280
281 template<typename S2, int O2>
282 typename SE3GroupAction<Derived>::ReturnType
283 233066 se3ActionInverse_impl(const SE3Tpl<S2, O2> & m) const
284 {
285 233066 typename SE3GroupAction<Derived>::ReturnType res;
286 233066 se3ActionInverse_impl(m, res);
287 233066 return res;
288 }
289
290 12 void disp_impl(std::ostream & os) const
291 {
292
3/6
✓ Branch 3 taken 12 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 12 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 12 times.
✗ Branch 10 not taken.
 12 os << " v = " << linear().transpose() << std::endl
293
5/10
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 12 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 12 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 12 times.
✗ Branch 14 not taken.
 12 << " w = " << angular().transpose() << std::endl;
294 12 }
295
296 /// \returns a MotionRef on this.
297 MotionRefType ref()
298 {
299 return derived().ref();
300 }
301
302 protected:
303 1011229 MotionDense() {};
304
305 MotionDense(const MotionDense &) = delete;
306
307 }; // class MotionDense
308
309 /// Basic operations specialization
310 template<typename M1, typename M2>
311 39000 typename traits<M1>::MotionPlain operator^(const MotionDense<M1> & v1, const MotionDense<M2> & v2)
312 {
313 39000 return v1.derived().cross(v2.derived());
314 }
315
316 template<typename M1, typename F1>
317 ✗ typename traits<F1>::ForcePlain operator^(const MotionDense<M1> & v, const ForceBase<F1> & f)
318 {
319 ✗ return v.derived().cross(f.derived());
320 }
321
322 template<typename M1>
323 typename traits<M1>::MotionPlain
324 180 operator*(const typename traits<M1>::Scalar alpha, const MotionDense<M1> & v)
325 {
326 180 return v * alpha;
327 }
328
329 } // namespace pinocchio
330
331 #endif // ifndef __pinocchio_spatial_motion_dense_hpp__
332