Directory: | ./ |
---|---|
File: | include/pinocchio/spatial/motion-tpl.hpp |
Date: | 2024-08-27 18:20:05 |
Exec | Total | Coverage | |
---|---|---|---|
Lines: | 51 | 64 | 79.7% |
Branches: | 9 | 37 | 24.3% |
Line | Branch | Exec | Source |
---|---|---|---|
1 | // | ||
2 | // Copyright (c) 2015-2018 CNRS | ||
3 | // Copyright (c) 2015-2016 Wandercraft, 86 rue de Paris 91400 Orsay, France. | ||
4 | // | ||
5 | |||
6 | #ifndef __pinocchio_spatial_motion_tpl_hpp__ | ||
7 | #define __pinocchio_spatial_motion_tpl_hpp__ | ||
8 | |||
9 | namespace pinocchio | ||
10 | { | ||
11 | template<typename _Scalar, int _Options> | ||
12 | struct traits<MotionTpl<_Scalar, _Options>> | ||
13 | { | ||
14 | typedef _Scalar Scalar; | ||
15 | typedef Eigen::Matrix<Scalar, 3, 1, _Options> Vector3; | ||
16 | typedef Eigen::Matrix<Scalar, 6, 1, _Options> Vector6; | ||
17 | typedef Eigen::Matrix<Scalar, 4, 4, _Options> Matrix4; | ||
18 | typedef Eigen::Matrix<Scalar, 6, 6, _Options> Matrix6; | ||
19 | typedef Matrix6 ActionMatrixType; | ||
20 | typedef Matrix4 HomogeneousMatrixType; | ||
21 | typedef typename PINOCCHIO_EIGEN_REF_CONST_TYPE(Vector6) ToVectorConstReturnType; | ||
22 | typedef typename PINOCCHIO_EIGEN_REF_TYPE(Vector6) ToVectorReturnType; | ||
23 | typedef typename Vector6::template FixedSegmentReturnType<3>::Type LinearType; | ||
24 | typedef typename Vector6::template FixedSegmentReturnType<3>::Type AngularType; | ||
25 | typedef typename Vector6::template ConstFixedSegmentReturnType<3>::Type ConstLinearType; | ||
26 | typedef typename Vector6::template ConstFixedSegmentReturnType<3>::Type ConstAngularType; | ||
27 | typedef MotionTpl<Scalar, _Options> MotionPlain; | ||
28 | typedef const MotionPlain & PlainReturnType; | ||
29 | enum | ||
30 | { | ||
31 | LINEAR = 0, | ||
32 | ANGULAR = 3, | ||
33 | Options = _Options | ||
34 | }; | ||
35 | |||
36 | typedef MotionRef<Vector6> MotionRefType; | ||
37 | }; // traits MotionTpl | ||
38 | |||
39 | template<typename _Scalar, int _Options> | ||
40 | class MotionTpl : public MotionDense<MotionTpl<_Scalar, _Options>> | ||
41 | { | ||
42 | public: | ||
43 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW | ||
44 | typedef MotionDense<MotionTpl> Base; | ||
45 | MOTION_TYPEDEF_TPL(MotionTpl); | ||
46 | enum | ||
47 | { | ||
48 | Options = _Options | ||
49 | }; | ||
50 | |||
51 | using Base::operator=; | ||
52 | using Base::angular; | ||
53 | using Base::linear; | ||
54 | |||
55 | using Base::__mequ__; | ||
56 | using Base::__minus__; | ||
57 | using Base::__mult__; | ||
58 | using Base::__opposite__; | ||
59 | using Base::__pequ__; | ||
60 | using Base::__plus__; | ||
61 | |||
62 | // Constructors | ||
63 | 198492 | MotionTpl() | |
64 | 198492 | { | |
65 | 198492 | } | |
66 | |||
67 | template<typename V1, typename V2> | ||
68 | 6739 | MotionTpl(const Eigen::MatrixBase<V1> & v, const Eigen::MatrixBase<V2> & w) | |
69 | 6739 | { | |
70 | EIGEN_STATIC_ASSERT_VECTOR_ONLY(V1); | ||
71 | EIGEN_STATIC_ASSERT_VECTOR_ONLY(V2); | ||
72 |
1/2✓ Branch 2 taken 3522 times.
✗ Branch 3 not taken.
|
6739 | linear() = v; |
73 |
1/2✓ Branch 2 taken 3522 times.
✗ Branch 3 not taken.
|
6739 | angular() = w; |
74 | 6739 | } | |
75 | |||
76 | template<typename V6> | ||
77 | 127554 | explicit MotionTpl(const Eigen::MatrixBase<V6> & v) | |
78 | 127554 | : m_data(v) | |
79 | { | ||
80 | EIGEN_STATIC_ASSERT_VECTOR_ONLY(V6); | ||
81 | 127554 | } | |
82 | |||
83 | 127191 | MotionTpl(const MotionTpl & other) | |
84 | 127191 | { | |
85 | 127191 | *this = other; | |
86 | 127191 | } | |
87 | |||
88 | template<typename S2, int O2> | ||
89 | ✗ | explicit MotionTpl(const MotionTpl<S2, O2> & other) | |
90 | { | ||
91 | ✗ | *this = other.template cast<Scalar>(); | |
92 | } | ||
93 | |||
94 | template<int O2> | ||
95 | explicit MotionTpl(const MotionTpl<Scalar, O2> & clone) | ||
96 | : m_data(clone.toVector()) | ||
97 | { | ||
98 | } | ||
99 | |||
100 | // Same explanation as converting constructor from MotionBase | ||
101 | template< | ||
102 | typename M2, | ||
103 | typename std::enable_if<!std::is_convertible<MotionDense<M2>, MotionTpl>::value, bool>::type = | ||
104 | true> | ||
105 | explicit MotionTpl(const MotionDense<M2> & clone) | ||
106 | { | ||
107 | linear() = clone.linear(); | ||
108 | angular() = clone.angular(); | ||
109 | } | ||
110 | |||
111 | // MotionBase implement a conversion function to PlainReturnType. | ||
112 | // Usually, PlainReturnType is defined as MotionTpl. | ||
113 | // In this case, this converting constructor is redundant and | ||
114 | // create a warning with -Wconversion | ||
115 | template< | ||
116 | typename M2, | ||
117 | typename std::enable_if<!std::is_convertible<MotionBase<M2>, MotionTpl>::value, bool>::type = | ||
118 | true> | ||
119 | explicit MotionTpl(const MotionBase<M2> & clone) | ||
120 | { | ||
121 | *this = clone; | ||
122 | } | ||
123 | |||
124 | /// | ||
125 | /// \brief Copy assignment operator. | ||
126 | /// | ||
127 | /// \param[in] other MotionTpl to copy | ||
128 | /// | ||
129 | 258458 | MotionTpl & operator=(const MotionTpl & clone) // Copy assignment operator | |
130 | { | ||
131 | 258458 | m_data = clone.toVector(); | |
132 | 258458 | return *this; | |
133 | } | ||
134 | |||
135 | // initializers | ||
136 | 7047 | static MotionTpl Zero() | |
137 | { | ||
138 |
1/2✓ Branch 2 taken 7047 times.
✗ Branch 3 not taken.
|
7047 | return MotionTpl(Vector6::Zero()); |
139 | } | ||
140 | ✗ | static MotionTpl Random() | |
141 | { | ||
142 | ✗ | return MotionTpl(Vector6::Random()); | |
143 | } | ||
144 | |||
145 | 30368 | inline PlainReturnType plain() const | |
146 | { | ||
147 | 30368 | return *this; | |
148 | } | ||
149 | |||
150 | 489276 | ToVectorConstReturnType toVector_impl() const | |
151 | { | ||
152 | 489276 | return m_data; | |
153 | } | ||
154 | 15429 | ToVectorReturnType toVector_impl() | |
155 | { | ||
156 | 15429 | return m_data; | |
157 | } | ||
158 | |||
159 | // Getters | ||
160 | 650092 | ConstAngularType angular_impl() const | |
161 | { | ||
162 | 650092 | return m_data.template segment<3>(ANGULAR); | |
163 | } | ||
164 | 372403 | ConstLinearType linear_impl() const | |
165 | { | ||
166 | 372403 | return m_data.template segment<3>(LINEAR); | |
167 | } | ||
168 | 375402 | AngularType angular_impl() | |
169 | { | ||
170 | 375402 | return m_data.template segment<3>(ANGULAR); | |
171 | } | ||
172 | 296549 | LinearType linear_impl() | |
173 | { | ||
174 | 296549 | return m_data.template segment<3>(LINEAR); | |
175 | } | ||
176 | |||
177 | template<typename V3> | ||
178 | ✗ | void angular_impl(const Eigen::MatrixBase<V3> & w) | |
179 | { | ||
180 | EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(V3, 3); | ||
181 | ✗ | angular_impl() = w; | |
182 | } | ||
183 | template<typename V3> | ||
184 | ✗ | void linear_impl(const Eigen::MatrixBase<V3> & v) | |
185 | { | ||
186 | EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(V3, 3); | ||
187 | ✗ | linear_impl() = v; | |
188 | } | ||
189 | |||
190 | // Specific operators for MotionTpl and MotionRef | ||
191 | template<int O2> | ||
192 | 559 | MotionPlain __plus__(const MotionTpl<Scalar, O2> & v) const | |
193 | { | ||
194 |
1/2✓ Branch 3 taken 559 times.
✗ Branch 4 not taken.
|
559 | return MotionPlain(m_data + v.toVector()); |
195 | } | ||
196 | |||
197 | template<typename Vector6ArgType> | ||
198 | 51 | MotionPlain __plus__(const MotionRef<Vector6ArgType> & v) const | |
199 | { | ||
200 |
2/5✓ Branch 2 taken 51 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 51 times.
✗ Branch 6 not taken.
|
51 | return MotionPlain(m_data + v.toVector()); |
201 | } | ||
202 | |||
203 | template<int O2> | ||
204 | 46322 | MotionPlain __minus__(const MotionTpl<Scalar, O2> & v) const | |
205 | { | ||
206 |
1/2✓ Branch 3 taken 46322 times.
✗ Branch 4 not taken.
|
46322 | return MotionPlain(m_data - v.toVector()); |
207 | } | ||
208 | |||
209 | template<typename Vector6ArgType> | ||
210 | MotionPlain __minus__(const MotionRef<Vector6ArgType> & v) const | ||
211 | { | ||
212 | return MotionPlain(m_data - v.toVector()); | ||
213 | } | ||
214 | |||
215 | template<int O2> | ||
216 | 215557 | MotionTpl & __pequ__(const MotionTpl<Scalar, O2> & v) | |
217 | { | ||
218 | 215557 | m_data += v.toVector(); | |
219 | 215557 | return *this; | |
220 | } | ||
221 | |||
222 | template<typename Vector6ArgType> | ||
223 | 367 | MotionTpl & __pequ__(const MotionRef<Vector6ArgType> & v) | |
224 | { | ||
225 |
1/2✓ Branch 2 taken 367 times.
✗ Branch 3 not taken.
|
367 | m_data += v.toVector(); |
226 | 367 | return *this; | |
227 | } | ||
228 | |||
229 | template<int O2> | ||
230 | 323 | MotionTpl & __mequ__(const MotionTpl<Scalar, O2> & v) | |
231 | { | ||
232 | 323 | m_data -= v.toVector(); | |
233 | 323 | return *this; | |
234 | } | ||
235 | |||
236 | template<typename Vector6ArgType> | ||
237 | MotionTpl & __mequ__(const MotionRef<Vector6ArgType> & v) | ||
238 | { | ||
239 | m_data -= v.toVector(); | ||
240 | return *this; | ||
241 | } | ||
242 | |||
243 | template<typename OtherScalar> | ||
244 | 10597 | MotionPlain __mult__(const OtherScalar & alpha) const | |
245 | { | ||
246 |
1/2✓ Branch 2 taken 10597 times.
✗ Branch 3 not taken.
|
10597 | return MotionPlain(alpha * m_data); |
247 | } | ||
248 | |||
249 | ✗ | MotionRef<Vector6> ref() | |
250 | { | ||
251 | ✗ | return MotionRef<Vector6>(m_data); | |
252 | } | ||
253 | |||
254 | /// \returns An expression of *this with the Scalar type casted to NewScalar. | ||
255 | template<typename NewScalar> | ||
256 | ✗ | MotionTpl<NewScalar, Options> cast() const | |
257 | { | ||
258 | typedef MotionTpl<NewScalar, Options> ReturnType; | ||
259 | ✗ | ReturnType res(linear().template cast<NewScalar>(), angular().template cast<NewScalar>()); | |
260 | ✗ | return res; | |
261 | } | ||
262 | |||
263 | protected: | ||
264 | Vector6 m_data; | ||
265 | |||
266 | }; // class MotionTpl | ||
267 | |||
268 | } // namespace pinocchio | ||
269 | |||
270 | #endif // ifndef __pinocchio_spatial_motion_tpl_hpp__ | ||
271 |