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 |