GCC Code Coverage Report

Directory: ./
File: include/pinocchio/spatial/motion-dense.hpp
Date: 2025-04-30 16:14:33
Exec Total Coverage
Lines: 111 119 93.3%
Branches: 127 311 40.8%
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 106929 Derived & setZero()
40 {
41
1/2
✓ Branch 2 taken 53649 times.
✗ Branch 3 not taken.
 106929 linear().setZero();
42
1/2
✓ Branch 2 taken 53649 times.
✗ Branch 3 not taken.
 106929 angular().setZero();
43 106929 return derived();
44 }
45 4 Derived & setRandom()
46 {
47
1/2
✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
 6 linear().setRandom();
48
1/2
✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
 6 angular().setRandom();
49 6 return derived();
50 }
51
52 161 ActionMatrixType toActionMatrix_impl() const
53 {
54 161 ActionMatrixType X;
55
5/10
✓ Branch 1 taken 161 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 161 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 161 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 161 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 161 times.
✗ Branch 14 not taken.
 161 X.template block<3, 3>(ANGULAR, ANGULAR) = X.template block<3, 3>(LINEAR, LINEAR) =
56
0/2
✗ Branch 1 not taken.
✗ Branch 2 not taken.
 161 skew(angular());
57
3/11
✗ Branch 1 not taken.
✓ Branch 2 taken 161 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 161 times.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✓ Branch 8 taken 161 times.
✗ Branch 9 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
 161 X.template block<3, 3>(LINEAR, ANGULAR) = skew(linear());
58
1/5
✗ Branch 1 not taken.
✓ Branch 2 taken 161 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
 161 X.template block<3, 3>(ANGULAR, LINEAR).setZero();
59
60 161 return X;
61 }
62
63 6 ActionMatrixType toDualActionMatrix_impl() const
64 {
65 6 ActionMatrixType X;
66
5/10
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 6 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 6 times.
✗ Branch 14 not taken.
 6 X.template block<3, 3>(ANGULAR, ANGULAR) = X.template block<3, 3>(LINEAR, LINEAR) =
67
0/2
✗ Branch 1 not taken.
✗ Branch 2 not taken.
 6 skew(angular());
68
3/11
✗ Branch 1 not taken.
✓ Branch 2 taken 6 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 6 times.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✓ Branch 8 taken 6 times.
✗ Branch 9 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
 6 X.template block<3, 3>(ANGULAR, LINEAR) = skew(linear());
69
1/5
✗ Branch 1 not taken.
✓ Branch 2 taken 6 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
 6 X.template block<3, 3>(LINEAR, ANGULAR).setZero();
70
71 6 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 3563 bool isEqual_impl(const MotionDense<D2> & other) const
85 {
86
8/16
✓ Branch 1 taken 3527 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3527 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3527 times.
✗ Branch 8 not taken.
✓ Branch 9 taken 3527 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 3527 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 3527 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 3527 times.
✗ Branch 19 not taken.
✓ Branch 20 taken 3527 times.
✗ Branch 21 not taken.
 3563 return linear() == other.linear() && angular() == other.angular();
87 }
88
89 template<typename D2>
90 44 bool isEqual_impl(const MotionBase<D2> & other) const
91 {
92 44 return other.derived() == derived();
93 }
94
95 // Arithmetic operators
96 template<typename D2>
97 46771 Derived & operator=(const MotionDense<D2> & other)
98 {
99 46771 return derived().set(other.derived());
100 }
101
102 10 Derived & operator=(const MotionDense & other)
103 {
104 10 return derived().set(other.derived());
105 }
106
107 template<typename D2>
108 46781 Derived & set(const MotionDense<D2> & other)
109 {
110
2/4
✓ Branch 2 taken 26477 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 26477 times.
✗ Branch 6 not taken.
 46781 linear() = other.linear();
111
2/4
✓ Branch 2 taken 26477 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 26477 times.
✗ Branch 6 not taken.
 46781 angular() = other.angular();
112 46781 return derived();
113 }
114
115 template<typename D2>
116 243156 Derived & operator=(const MotionBase<D2> & other)
117 {
118 243156 other.derived().setTo(derived());
119 243156 return derived();
120 }
121
122 template<typename V6>
123 7522 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 7404 times.
 7522 assert(v.size() == 6);
127
2/4
✓ Branch 2 taken 7404 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 7404 times.
✗ Branch 6 not taken.
 7522 linear() = v.template segment<3>(LINEAR);
128
2/4
✓ Branch 2 taken 7404 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 7404 times.
✗ Branch 6 not taken.
 7522 angular() = v.template segment<3>(ANGULAR);
129 7522 return derived();
130 }
131
132 5228 MotionPlain operator-() const
133 {
134 5228 return derived().__opposite__();
135 }
136 template<typename M1>
137 105173 MotionPlain operator+(const MotionDense<M1> & v) const
138 {
139 105173 return derived().__plus__(v.derived());
140 }
141 template<typename M1>
142 47746 MotionPlain operator-(const MotionDense<M1> & v) const
143 {
144 47746 return derived().__minus__(v.derived());
145 }
146
147 template<typename M1>
148 559614 Derived & operator+=(const MotionDense<M1> & v)
149 {
150 559614 return derived().__pequ__(v.derived());
151 }
152 template<typename M1>
153 105010 Derived & operator+=(const MotionBase<M1> & v)
154 {
155 105010 v.derived().addTo(derived());
156 105010 return derived();
157 }
158
159 template<typename M1>
160 585 Derived & operator-=(const MotionDense<M1> & v)
161 {
162 585 return derived().__mequ__(v.derived());
163 }
164
165 5228 MotionPlain __opposite__() const
166 {
167
4/8
✓ Branch 2 taken 5215 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 5215 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 5215 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 5215 times.
✗ Branch 12 not taken.
 5228 return MotionPlain(-linear(), -angular());
168 }
169
170 template<typename M1>
171 128 MotionPlain __plus__(const MotionDense<M1> & v) const
172 {
173
6/12
✓ Branch 2 taken 128 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 128 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 128 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 128 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 128 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 128 times.
✗ Branch 18 not taken.
 128 return MotionPlain(linear() + v.linear(), angular() + v.angular());
174 }
175
176 template<typename M1>
177 1 MotionPlain __minus__(const MotionDense<M1> & v) const
178 {
179
6/12
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
 1 return MotionPlain(linear() - v.linear(), angular() - v.angular());
180 }
181
182 template<typename M1>
183 164 Derived & __pequ__(const MotionDense<M1> & v)
184 {
185
2/4
✓ Branch 2 taken 164 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 164 times.
✗ Branch 6 not taken.
 164 linear() += v.linear();
186
2/4
✓ Branch 2 taken 164 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 164 times.
✗ Branch 6 not taken.
 164 angular() += v.angular();
187 164 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 4 MotionPlain __div__(const OtherScalar & alpha) const
206 {
207
1/5
✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
 8 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 295326 typename MotionAlgebraAction<D, Derived>::ReturnType cross_impl(const D & d) const
218 {
219 295326 return d.motionAction(derived());
220 }
221
222 template<typename M1, typename M2>
223 135294 void motionAction(const MotionDense<M1> & v, MotionDense<M2> & mout) const
224 {
225
8/16
✓ Branch 2 taken 88744 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 88744 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 88744 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 88744 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 88744 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 88744 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 88744 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 88744 times.
✗ Branch 24 not taken.
 135294 mout.linear() = v.linear().cross(angular()) + v.angular().cross(linear());
226
4/8
✓ Branch 2 taken 88744 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 88744 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 88744 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 88744 times.
✗ Branch 12 not taken.
 135294 mout.angular() = v.angular().cross(angular());
227 135294 }
228
229 template<typename M1>
230 60392 MotionPlain motionAction(const MotionDense<M1> & v) const
231 {
232 60392 MotionPlain res;
233
1/2
✓ Branch 1 taken 144 times.
✗ Branch 2 not taken.
 60392 motionAction(v, res);
234 60392 return res;
235 }
236
237 template<typename M2>
238 16 bool isApprox(
239 const MotionDense<M2> & m2,
240 const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const
241 {
242 16 return derived().isApprox_impl(m2, prec);
243 }
244
245 template<typename D2>
246 4954 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 4915 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4915 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4915 times.
✗ Branch 8 not taken.
✓ Branch 9 taken 4915 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 4915 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 4915 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 4915 times.
✗ Branch 19 not taken.
✓ Branch 20 taken 4915 times.
✗ Branch 21 not taken.
 4954 return linear().isApprox(m2.linear(), prec) && angular().isApprox(m2.angular(), prec);
251 }
252
253 101 bool isZero_impl(const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const
254 {
255
7/12
✓ Branch 1 taken 101 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 101 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 48 times.
✓ Branch 7 taken 53 times.
✓ Branch 9 taken 48 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 48 times.
✗ Branch 13 not taken.
✓ Branch 14 taken 48 times.
✗ Branch 15 not taken.
 101 return linear().isZero(prec) && angular().isZero(prec);
256 }
257
258 template<typename S2, int O2, typename D2>
259 96712 void se3Action_impl(const SE3Tpl<S2, O2> & m, MotionDense<D2> & v) const
260 {
261
5/10
✓ Branch 2 taken 61892 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 61892 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 61892 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 61892 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 61892 times.
✗ Branch 15 not taken.
 96712 v.angular().noalias() = m.rotation() * angular();
262
8/16
✓ Branch 3 taken 61892 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 61892 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 61892 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 61892 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 61892 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 61892 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 61892 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 61892 times.
✗ Branch 25 not taken.
 96712 v.linear().noalias() = m.rotation() * linear() + m.translation().cross(v.angular());
263 96712 }
264
265 template<typename S2, int O2>
266 96712 typename SE3GroupAction<Derived>::ReturnType se3Action_impl(const SE3Tpl<S2, O2> & m) const
267 {
268 96712 typename SE3GroupAction<Derived>::ReturnType res;
269
1/2
✓ Branch 1 taken 56 times.
✗ Branch 2 not taken.
 96712 se3Action_impl(m, res);
270 96712 return res;
271 }
272
273 template<typename S2, int O2, typename D2>
274 394173 void se3ActionInverse_impl(const SE3Tpl<S2, O2> & m, MotionDense<D2> & v) const
275 {
276
7/14
✓ Branch 1 taken 197735 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 197735 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 197735 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 197735 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 197735 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 197735 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 30 times.
✗ Branch 20 not taken.
 1182459 v.linear().noalias() =
277
3/6
✓ Branch 3 taken 197735 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 197735 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 197705 times.
✗ Branch 10 not taken.
 1183546 m.rotation().transpose() * (linear() - m.translation().cross(angular()));
278
6/12
✓ Branch 2 taken 197735 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 197735 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 197735 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 197735 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 197735 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 197735 times.
✗ Branch 18 not taken.
 394173 v.angular().noalias() = m.rotation().transpose() * angular();
279 394173 }
280
281 template<typename S2, int O2>
282 typename SE3GroupAction<Derived>::ReturnType
283 394173 se3ActionInverse_impl(const SE3Tpl<S2, O2> & m) const
284 {
285 394173 typename SE3GroupAction<Derived>::ReturnType res;
286
1/2
✓ Branch 1 taken 691 times.
✗ Branch 2 not taken.
 394173 se3ActionInverse_impl(m, res);
287 394173 return res;
288 }
289
290 49 void disp_impl(std::ostream & os) const
291 {
292
3/6
✓ Branch 3 taken 49 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 49 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 49 times.
✗ Branch 10 not taken.
 49 os << " v = " << linear().transpose() << std::endl
293
5/10
✓ Branch 1 taken 49 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 49 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 49 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 49 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 49 times.
✗ Branch 14 not taken.
 49 << " w = " << angular().transpose() << std::endl;
294 49 }
295
296 /// \returns a MotionRef on this.
297 MotionRefType ref()
298 {
299 return derived().ref();
300 }
301
302 protected:
303 3799054 MotionDense() {};
304
305 MotionDense(const MotionDense &) = delete;
306
307 }; // class MotionDense
308
309 /// Basic operations specialization
310 template<typename M1, typename M2>
311 41454 typename traits<M1>::MotionPlain operator^(const MotionDense<M1> & v1, const MotionDense<M2> & v2)
312 {
313 41454 return v1.derived().cross(v2.derived());
314 }
315
316 template<typename M1, typename F1>
317 3 typename traits<F1>::ForcePlain operator^(const MotionDense<M1> & v, const ForceBase<F1> & f)
318 {
319 3 return v.derived().cross(f.derived());
320 }
321
322 template<typename M1>
323 typename traits<M1>::MotionPlain
324 15880 operator*(const typename traits<M1>::Scalar alpha, const MotionDense<M1> & v)
325 {
326 15880 return v * alpha;
327 }
328
329 } // namespace pinocchio
330
331 #endif // ifndef __pinocchio_spatial_motion_dense_hpp__
332