Directory: | ./ |
---|---|
File: | include/pinocchio/spatial/motion-ref.hpp |
Date: | 2024-08-27 18:20:05 |
Exec | Total | Coverage | |
---|---|---|---|
Lines: | 33 | 42 | 78.6% |
Branches: | 3 | 8 | 37.5% |
Line | Branch | Exec | Source |
---|---|---|---|
1 | // | ||
2 | // Copyright (c) 2017-2019 CNRS INRIA | ||
3 | // | ||
4 | |||
5 | #ifndef __pinocchio_spatial_motion_ref_hpp__ | ||
6 | #define __pinocchio_spatial_motion_ref_hpp__ | ||
7 | |||
8 | namespace pinocchio | ||
9 | { | ||
10 | |||
11 | template<typename Vector6ArgType> | ||
12 | struct traits<MotionRef<Vector6ArgType>> | ||
13 | { | ||
14 | typedef typename Vector6ArgType::Scalar Scalar; | ||
15 | typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(Vector6ArgType) Vector6; | ||
16 | enum | ||
17 | { | ||
18 | LINEAR = 0, | ||
19 | ANGULAR = 3, | ||
20 | Options = Vector6::Options | ||
21 | }; | ||
22 | typedef Eigen::Matrix<Scalar, 3, 1, Options> Vector3; | ||
23 | typedef Eigen::Matrix<Scalar, 4, 4, Options> Matrix4; | ||
24 | typedef Eigen::Matrix<Scalar, 6, 6, Options> Matrix6; | ||
25 | typedef Matrix6 ActionMatrixType; | ||
26 | typedef Matrix4 HomogeneousMatrixType; | ||
27 | typedef typename Vector6ArgType::template FixedSegmentReturnType<3>::Type LinearType; | ||
28 | typedef typename Vector6ArgType::template FixedSegmentReturnType<3>::Type AngularType; | ||
29 | typedef typename Vector6ArgType::template ConstFixedSegmentReturnType<3>::Type ConstLinearType; | ||
30 | typedef typename Vector6ArgType::template ConstFixedSegmentReturnType<3>::Type ConstAngularType; | ||
31 | typedef MotionTpl<Scalar, Options> MotionPlain; | ||
32 | typedef MotionPlain PlainReturnType; | ||
33 | typedef typename PINOCCHIO_EIGEN_REF_TYPE(Vector6ArgType) DataRefType; | ||
34 | typedef DataRefType ToVectorReturnType; | ||
35 | typedef typename PINOCCHIO_EIGEN_REF_CONST_TYPE(Vector6ArgType) ConstDataRefType; | ||
36 | typedef ConstDataRefType ToVectorConstReturnType; | ||
37 | typedef MotionRef<Vector6ArgType> MotionRefType; | ||
38 | |||
39 | }; // traits MotionRef | ||
40 | |||
41 | template<typename Vector6ArgType> | ||
42 | struct SE3GroupAction<MotionRef<Vector6ArgType>> | ||
43 | { | ||
44 | typedef typename traits<MotionRef<Vector6ArgType>>::MotionPlain ReturnType; | ||
45 | }; | ||
46 | |||
47 | template<typename Vector6ArgType, typename MotionDerived> | ||
48 | struct MotionAlgebraAction<MotionRef<Vector6ArgType>, MotionDerived> | ||
49 | { | ||
50 | typedef typename traits<MotionRef<Vector6ArgType>>::MotionPlain ReturnType; | ||
51 | }; | ||
52 | |||
53 | namespace internal | ||
54 | { | ||
55 | template<typename Vector6ArgType, typename Scalar> | ||
56 | struct RHSScalarMultiplication<MotionRef<Vector6ArgType>, Scalar> | ||
57 | { | ||
58 | typedef typename pinocchio::traits<MotionRef<Vector6ArgType>>::MotionPlain ReturnType; | ||
59 | }; | ||
60 | |||
61 | template<typename Vector6ArgType, typename Scalar> | ||
62 | struct LHSScalarMultiplication<MotionRef<Vector6ArgType>, Scalar> | ||
63 | { | ||
64 | typedef typename traits<MotionRef<Vector6ArgType>>::MotionPlain ReturnType; | ||
65 | }; | ||
66 | } // namespace internal | ||
67 | |||
68 | template<typename Vector6ArgType> | ||
69 | class MotionRef : public MotionDense<MotionRef<Vector6ArgType>> | ||
70 | { | ||
71 | public: | ||
72 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW | ||
73 | typedef MotionDense<MotionRef> Base; | ||
74 | typedef typename traits<MotionRef>::DataRefType DataRefType; | ||
75 | MOTION_TYPEDEF_TPL(MotionRef); | ||
76 | |||
77 | using Base::operator=; | ||
78 | using Base::angular; | ||
79 | using Base::linear; | ||
80 | |||
81 | using Base::__mequ__; | ||
82 | using Base::__minus__; | ||
83 | using Base::__mult__; | ||
84 | using Base::__opposite__; | ||
85 | using Base::__pequ__; | ||
86 | using Base::__plus__; | ||
87 | |||
88 | /// \brief Default constructor from a 6 dimensional vector. | ||
89 | 70928 | MotionRef(typename PINOCCHIO_EIGEN_REF_TYPE(Vector6ArgType) v_like) | |
90 | 70928 | : m_ref(v_like) | |
91 | { | ||
92 | EIGEN_STATIC_ASSERT( | ||
93 | Vector6ArgType::ColsAtCompileTime == 1, YOU_TRIED_CALLING_A_VECTOR_METHOD_ON_A_MATRIX); | ||
94 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 35464 times.
|
70928 | assert(v_like.size() == 6); |
95 | 70928 | } | |
96 | |||
97 | /// \brief Copy constructor from another MotionRef. | ||
98 | MotionRef(const MotionRef & other) | ||
99 | : m_ref(other.m_ref) | ||
100 | { | ||
101 | } | ||
102 | |||
103 | 51 | ToVectorConstReturnType toVector_impl() const | |
104 | { | ||
105 | 51 | return m_ref; | |
106 | } | ||
107 | 27 | ToVectorReturnType toVector_impl() | |
108 | { | ||
109 | 27 | return m_ref; | |
110 | } | ||
111 | |||
112 | // Getters | ||
113 | 13084 | ConstAngularType angular_impl() const | |
114 | { | ||
115 | 13084 | return ConstAngularType(m_ref.derived(), ANGULAR); | |
116 | } | ||
117 | 4159 | ConstLinearType linear_impl() const | |
118 | { | ||
119 | 8106 | return ConstLinearType(m_ref.derived(), LINEAR); | |
120 | } | ||
121 | 44486 | AngularType angular_impl() | |
122 | { | ||
123 | 50294 | return m_ref.template segment<3>(ANGULAR); | |
124 | } | ||
125 | 26022 | LinearType linear_impl() | |
126 | { | ||
127 | 51969 | return m_ref.template segment<3>(LINEAR); | |
128 | } | ||
129 | |||
130 | template<typename V3> | ||
131 | void angular_impl(const Eigen::MatrixBase<V3> & w) | ||
132 | { | ||
133 | EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(V3, 3); | ||
134 | angular_impl() = w; | ||
135 | } | ||
136 | |||
137 | template<typename V3> | ||
138 | void linear_impl(const Eigen::MatrixBase<V3> & v) | ||
139 | { | ||
140 | EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(V3, 3); | ||
141 | linear_impl() = v; | ||
142 | } | ||
143 | |||
144 | // Specific operators for MotionTpl and MotionRef | ||
145 | template<typename S1, int O1> | ||
146 | MotionPlain __plus__(const MotionTpl<S1, O1> & v) const | ||
147 | { | ||
148 | return MotionPlain(m_ref + v.toVector()); | ||
149 | } | ||
150 | |||
151 | template<typename Vector6Like> | ||
152 | MotionPlain __plus__(const MotionRef<Vector6ArgType> & v) const | ||
153 | { | ||
154 | return MotionPlain(m_ref + v.toVector()); | ||
155 | } | ||
156 | |||
157 | template<typename S1, int O1> | ||
158 | MotionPlain __minus__(const MotionTpl<S1, O1> & v) const | ||
159 | { | ||
160 | return MotionPlain(m_ref - v.toVector()); | ||
161 | } | ||
162 | |||
163 | template<typename Vector6Like> | ||
164 | MotionPlain __minus__(const MotionRef<Vector6ArgType> & v) const | ||
165 | { | ||
166 | return MotionPlain(m_ref - v.toVector()); | ||
167 | } | ||
168 | |||
169 | template<typename S1, int O1> | ||
170 | 8556 | MotionRef & __pequ__(const MotionTpl<S1, O1> & v) | |
171 | { | ||
172 | 8556 | m_ref += v.toVector(); | |
173 | 8556 | return *this; | |
174 | } | ||
175 | |||
176 | template<typename Vector6Like> | ||
177 | MotionRef & __pequ__(const MotionRef<Vector6ArgType> & v) | ||
178 | { | ||
179 | m_ref += v.toVector(); | ||
180 | return *this; | ||
181 | } | ||
182 | |||
183 | template<typename S1, int O1> | ||
184 | ✗ | MotionRef & __mequ__(const MotionTpl<S1, O1> & v) | |
185 | { | ||
186 | ✗ | m_ref -= v.toVector(); | |
187 | ✗ | return *this; | |
188 | } | ||
189 | |||
190 | template<typename Vector6Like> | ||
191 | MotionRef & __mequ__(const MotionRef<Vector6ArgType> & v) | ||
192 | { | ||
193 | m_ref -= v.toVector(); | ||
194 | return *this; | ||
195 | } | ||
196 | |||
197 | template<typename OtherScalar> | ||
198 | ✗ | MotionPlain __mult__(const OtherScalar & alpha) const | |
199 | { | ||
200 | ✗ | return MotionPlain(alpha * m_ref); | |
201 | } | ||
202 | |||
203 | ✗ | MotionRef & ref() | |
204 | { | ||
205 | ✗ | return *this; | |
206 | } | ||
207 | |||
208 | ✗ | inline PlainReturnType plain() const | |
209 | { | ||
210 | ✗ | return PlainReturnType(m_ref); | |
211 | } | ||
212 | |||
213 | protected: | ||
214 | DataRefType m_ref; | ||
215 | |||
216 | }; // class MotionRef<Vector6Like> | ||
217 | |||
218 | template<typename Vector6ArgType> | ||
219 | struct traits<MotionRef<const Vector6ArgType>> | ||
220 | { | ||
221 | typedef typename Vector6ArgType::Scalar Scalar; | ||
222 | typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(Vector6ArgType) Vector6; | ||
223 | enum | ||
224 | { | ||
225 | LINEAR = 0, | ||
226 | ANGULAR = 3, | ||
227 | Options = Vector6::Options | ||
228 | }; | ||
229 | typedef Eigen::Matrix<Scalar, 3, 1, Options> Vector3; | ||
230 | typedef Eigen::Matrix<Scalar, 4, 4, Options> Matrix4; | ||
231 | typedef Eigen::Matrix<Scalar, 6, 6, Options> Matrix6; | ||
232 | typedef Matrix6 ActionMatrixType; | ||
233 | typedef Matrix4 HomogeneousMatrixType; | ||
234 | typedef typename Vector6ArgType::template ConstFixedSegmentReturnType<3>::Type ConstLinearType; | ||
235 | typedef typename Vector6ArgType::template ConstFixedSegmentReturnType<3>::Type ConstAngularType; | ||
236 | typedef ConstLinearType LinearType; | ||
237 | typedef ConstAngularType AngularType; | ||
238 | typedef MotionTpl<Scalar, Options> MotionPlain; | ||
239 | typedef MotionPlain PlainReturnType; | ||
240 | typedef typename PINOCCHIO_EIGEN_REF_CONST_TYPE(Vector6ArgType) ConstDataRefType; | ||
241 | typedef ConstDataRefType ToVectorConstReturnType; | ||
242 | typedef ConstDataRefType DataRefType; | ||
243 | typedef DataRefType ToVectorReturnType; | ||
244 | typedef MotionRef<const Vector6ArgType> MotionRefType; | ||
245 | |||
246 | }; // traits MotionRef<const Vector6ArgType> | ||
247 | |||
248 | template<typename Vector6ArgType> | ||
249 | class MotionRef<const Vector6ArgType> : public MotionDense<MotionRef<const Vector6ArgType>> | ||
250 | { | ||
251 | public: | ||
252 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW | ||
253 | typedef MotionDense<MotionRef> Base; | ||
254 | typedef typename traits<MotionRef>::DataRefType DataRefType; | ||
255 | MOTION_TYPEDEF_TPL(MotionRef); | ||
256 | |||
257 | using Base::operator=; | ||
258 | using Base::angular; | ||
259 | using Base::linear; | ||
260 | |||
261 | using Base::__minus__; | ||
262 | using Base::__mult__; | ||
263 | using Base::__opposite__; | ||
264 | using Base::__plus__; | ||
265 | |||
266 | 284571 | MotionRef(typename PINOCCHIO_EIGEN_REF_CONST_TYPE(Vector6ArgType) v_like) | |
267 | 284571 | : m_ref(v_like) | |
268 | { | ||
269 | EIGEN_STATIC_ASSERT( | ||
270 | Vector6ArgType::ColsAtCompileTime == 1, YOU_TRIED_CALLING_A_VECTOR_METHOD_ON_A_MATRIX); | ||
271 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 143517 times.
|
284571 | assert(v_like.size() == 6); |
272 | 284571 | } | |
273 | |||
274 | /// \brief Copy constructor from another MotionRef. | ||
275 | MotionRef(const MotionRef & other) | ||
276 | : m_ref(other.m_ref) | ||
277 | { | ||
278 | } | ||
279 | |||
280 | 367 | ToVectorConstReturnType toVector_impl() const | |
281 | { | ||
282 | 367 | return m_ref; | |
283 | } | ||
284 | |||
285 | // Getters | ||
286 | 537038 | ConstAngularType angular_impl() const | |
287 | { | ||
288 | 537038 | return ConstAngularType(m_ref.derived(), ANGULAR); | |
289 | } | ||
290 | 3144 | ConstLinearType linear_impl() const | |
291 | { | ||
292 | 139738 | return ConstLinearType(m_ref.derived(), LINEAR); | |
293 | } | ||
294 | |||
295 | // Specific operators for MotionTpl and MotionRef | ||
296 | template<typename S1, int O1> | ||
297 | 1167 | MotionPlain __plus__(const MotionTpl<S1, O1> & v) const | |
298 | { | ||
299 |
1/2✓ Branch 3 taken 1167 times.
✗ Branch 4 not taken.
|
1167 | return MotionPlain(m_ref + v.toVector()); |
300 | } | ||
301 | |||
302 | template<typename Vector6Like> | ||
303 | MotionPlain __plus__(const MotionRef<Vector6ArgType> & v) const | ||
304 | { | ||
305 | return MotionPlain(m_ref + v.toVector()); | ||
306 | } | ||
307 | |||
308 | template<typename S1, int O1> | ||
309 | MotionPlain __minus__(const MotionTpl<S1, O1> & v) const | ||
310 | { | ||
311 | return MotionPlain(m_ref - v.toVector()); | ||
312 | } | ||
313 | |||
314 | template<typename Vector6Like> | ||
315 | MotionPlain __minus__(const MotionRef<Vector6ArgType> & v) const | ||
316 | { | ||
317 | return MotionPlain(m_ref - v.toVector()); | ||
318 | } | ||
319 | |||
320 | template<typename OtherScalar> | ||
321 | MotionPlain __mult__(const OtherScalar & alpha) const | ||
322 | { | ||
323 | return MotionPlain(alpha * m_ref); | ||
324 | } | ||
325 | |||
326 | const MotionRef & ref() const | ||
327 | { | ||
328 | return *this; | ||
329 | } | ||
330 | |||
331 | 4222 | inline PlainReturnType plain() const | |
332 | { | ||
333 | 4222 | return PlainReturnType(m_ref); | |
334 | } | ||
335 | |||
336 | protected: | ||
337 | DataRefType m_ref; | ||
338 | |||
339 | }; // class MotionRef<const Vector6Like> | ||
340 | |||
341 | } // namespace pinocchio | ||
342 | |||
343 | #endif // ifndef __pinocchio_spatial_motion_ref_hpp__ | ||
344 |