Directory: | ./ |
---|---|
File: | include/pinocchio/multibody/joint/joint-helical-unaligned.hpp |
Date: | 2024-08-27 18:20:05 |
Exec | Total | Coverage | |
---|---|---|---|
Lines: | 39 | 224 | 17.4% |
Branches: | 15 | 434 | 3.5% |
Line | Branch | Exec | Source |
---|---|---|---|
1 | // | ||
2 | // Copyright (c) 2022-2023 INRIA | ||
3 | // | ||
4 | |||
5 | #ifndef __pinocchio_multibody_joint_helical_unaligned_hpp__ | ||
6 | #define __pinocchio_multibody_joint_helical_unaligned_hpp__ | ||
7 | |||
8 | #include "pinocchio/fwd.hpp" | ||
9 | #include "pinocchio/multibody/joint/joint-base.hpp" | ||
10 | #include "pinocchio/multibody/joint-motion-subspace.hpp" | ||
11 | #include "pinocchio/spatial/inertia.hpp" | ||
12 | |||
13 | #include "pinocchio/math/matrix.hpp" | ||
14 | #include "pinocchio/math/rotation.hpp" | ||
15 | |||
16 | #include <iostream> | ||
17 | |||
18 | namespace pinocchio | ||
19 | { | ||
20 | |||
21 | template<typename Scalar, int Options> | ||
22 | struct MotionHelicalUnalignedTpl; | ||
23 | |||
24 | template<typename Scalar, int Options> | ||
25 | struct SE3GroupAction<MotionHelicalUnalignedTpl<Scalar, Options>> | ||
26 | { | ||
27 | typedef MotionTpl<Scalar, Options> ReturnType; | ||
28 | }; | ||
29 | |||
30 | template<typename Scalar, int Options, typename MotionDerived> | ||
31 | struct MotionAlgebraAction<MotionHelicalUnalignedTpl<Scalar, Options>, MotionDerived> | ||
32 | { | ||
33 | typedef MotionTpl<Scalar, Options> ReturnType; | ||
34 | }; | ||
35 | |||
36 | template<typename _Scalar, int _Options> | ||
37 | struct traits<MotionHelicalUnalignedTpl<_Scalar, _Options>> | ||
38 | { | ||
39 | typedef _Scalar Scalar; | ||
40 | enum | ||
41 | { | ||
42 | Options = _Options | ||
43 | }; | ||
44 | typedef Eigen::Matrix<Scalar, 3, 1, Options> Vector3; | ||
45 | typedef Eigen::Matrix<Scalar, 6, 1, Options> Vector6; | ||
46 | typedef Eigen::Matrix<Scalar, 4, 4, Options> Matrix4; | ||
47 | typedef Eigen::Matrix<Scalar, 6, 6, Options> Matrix6; | ||
48 | typedef typename PINOCCHIO_EIGEN_REF_CONST_TYPE(Vector6) ToVectorConstReturnType; | ||
49 | typedef typename PINOCCHIO_EIGEN_REF_TYPE(Vector6) ToVectorReturnType; | ||
50 | typedef Vector3 AngularType; | ||
51 | typedef Vector3 LinearType; | ||
52 | typedef const Vector3 ConstAngularType; | ||
53 | typedef const Vector3 ConstLinearType; | ||
54 | typedef Matrix6 ActionMatrixType; | ||
55 | typedef MotionTpl<Scalar, Options> MotionPlain; | ||
56 | typedef MotionPlain PlainReturnType; | ||
57 | typedef Matrix4 HomogeneousMatrixType; | ||
58 | enum | ||
59 | { | ||
60 | LINEAR = 0, | ||
61 | ANGULAR = 3 | ||
62 | }; | ||
63 | }; // traits MotionHelicalUnalignedTpl | ||
64 | |||
65 | template<typename _Scalar, int _Options> | ||
66 | struct MotionHelicalUnalignedTpl : MotionBase<MotionHelicalUnalignedTpl<_Scalar, _Options>> | ||
67 | { | ||
68 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW | ||
69 | |||
70 | MOTION_TYPEDEF_TPL(MotionHelicalUnalignedTpl); | ||
71 | |||
72 | ✗ | MotionHelicalUnalignedTpl() | |
73 | { | ||
74 | } | ||
75 | |||
76 | template<typename Vector3Like, typename OtherScalar> | ||
77 | 23 | MotionHelicalUnalignedTpl( | |
78 | const Eigen::MatrixBase<Vector3Like> & axis, const OtherScalar & w, const OtherScalar & v) | ||
79 | 23 | : m_axis(axis) | |
80 | 23 | , m_w(w) | |
81 | 23 | , m_v(v) | |
82 | { | ||
83 | 23 | } | |
84 | |||
85 | ✗ | inline PlainReturnType plain() const | |
86 | { | ||
87 | ✗ | return PlainReturnType(m_axis * m_v, m_axis * m_w); | |
88 | } | ||
89 | |||
90 | template<typename OtherScalar> | ||
91 | MotionHelicalUnalignedTpl __mult__(const OtherScalar & alpha) const | ||
92 | { | ||
93 | return MotionHelicalUnalignedTpl(m_axis, alpha * m_w, alpha * m_v); | ||
94 | } | ||
95 | |||
96 | template<typename MotionDerived> | ||
97 | ✗ | void setTo(MotionDense<MotionDerived> & m) const | |
98 | { | ||
99 | ✗ | for (Eigen::DenseIndex k = 0; k < 3; ++k) | |
100 | { | ||
101 | ✗ | m.angular().noalias() = m_axis * m_w; | |
102 | ✗ | m.linear().noalias() = m_axis * m_v; | |
103 | } | ||
104 | } | ||
105 | |||
106 | template<typename MotionDerived> | ||
107 | ✗ | inline void addTo(MotionDense<MotionDerived> & v) const | |
108 | { | ||
109 | ✗ | v.angular() += m_axis * m_w; | |
110 | ✗ | v.linear() += m_axis * m_v; | |
111 | } | ||
112 | |||
113 | template<typename S2, int O2, typename D2> | ||
114 | ✗ | inline void se3Action_impl(const SE3Tpl<S2, O2> & m, MotionDense<D2> & v) const | |
115 | { | ||
116 | ✗ | v.angular().noalias() = m_w * m.rotation() * m_axis; | |
117 | ✗ | v.linear().noalias() = m.translation().cross(v.angular()) + m_v * m.rotation() * m_axis; | |
118 | } | ||
119 | |||
120 | template<typename S2, int O2> | ||
121 | ✗ | MotionPlain se3Action_impl(const SE3Tpl<S2, O2> & m) const | |
122 | { | ||
123 | ✗ | MotionPlain res; | |
124 | ✗ | se3Action_impl(m, res); | |
125 | ✗ | return res; | |
126 | } | ||
127 | |||
128 | template<typename S2, int O2, typename D2> | ||
129 | ✗ | void se3ActionInverse_impl(const SE3Tpl<S2, O2> & m, MotionDense<D2> & v) const | |
130 | { | ||
131 | // Linear | ||
132 | ✗ | v.angular().noalias() = m_axis.cross(m.translation()); | |
133 | ✗ | v.angular() *= m_w; | |
134 | ✗ | v.linear().noalias() = | |
135 | ✗ | m.rotation().transpose() * v.angular() + m_v * (m.rotation().transpose() * m_axis); | |
136 | |||
137 | // Angular | ||
138 | ✗ | v.angular().noalias() = m.rotation().transpose() * m_axis * m_w; | |
139 | } | ||
140 | |||
141 | template<typename S2, int O2> | ||
142 | ✗ | MotionPlain se3ActionInverse_impl(const SE3Tpl<S2, O2> & m) const | |
143 | { | ||
144 | ✗ | MotionPlain res; | |
145 | ✗ | se3ActionInverse_impl(m, res); | |
146 | ✗ | return res; | |
147 | } | ||
148 | |||
149 | template<typename M1, typename M2> | ||
150 | ✗ | void motionAction(const MotionDense<M1> & v, MotionDense<M2> & mout) const | |
151 | { | ||
152 | // Linear | ||
153 | ✗ | mout.linear().noalias() = v.linear().cross(m_axis); | |
154 | ✗ | mout.linear() *= m_w; | |
155 | ✗ | mout.angular().noalias() = v.angular().cross(m_axis); | |
156 | ✗ | mout.angular() *= m_v; | |
157 | ✗ | mout.linear() += mout.angular(); | |
158 | |||
159 | // Angular | ||
160 | ✗ | mout.angular().noalias() = v.angular().cross(m_axis); | |
161 | ✗ | mout.angular() *= m_w; | |
162 | } | ||
163 | |||
164 | template<typename M1> | ||
165 | ✗ | MotionPlain motionAction(const MotionDense<M1> & v) const | |
166 | { | ||
167 | ✗ | MotionPlain res; | |
168 | ✗ | motionAction(v, res); | |
169 | ✗ | return res; | |
170 | } | ||
171 | |||
172 | ✗ | Scalar & angularRate() | |
173 | { | ||
174 | ✗ | return m_w; | |
175 | } | ||
176 | const Scalar & angularRate() const | ||
177 | { | ||
178 | return m_w; | ||
179 | } | ||
180 | |||
181 | ✗ | Scalar & linearRate() | |
182 | { | ||
183 | ✗ | return m_v; | |
184 | } | ||
185 | const Scalar & linearRate() const | ||
186 | { | ||
187 | return m_v; | ||
188 | } | ||
189 | |||
190 | ✗ | Vector3 & axis() | |
191 | { | ||
192 | ✗ | return m_axis; | |
193 | } | ||
194 | const Vector3 & axis() const | ||
195 | { | ||
196 | return m_axis; | ||
197 | } | ||
198 | |||
199 | ✗ | bool isEqual_impl(const MotionHelicalUnalignedTpl & other) const | |
200 | { | ||
201 | ✗ | return internal::comparison_eq(m_axis, other.m_axis) | |
202 | ✗ | && internal::comparison_eq(m_w, other.m_w) && internal::comparison_eq(m_v, other.m_v); | |
203 | } | ||
204 | |||
205 | protected: | ||
206 | Vector3 m_axis; | ||
207 | Scalar m_w, m_v; | ||
208 | |||
209 | }; // struct MotionHelicalUnalignedTpl | ||
210 | |||
211 | template<typename S1, int O1, typename MotionDerived> | ||
212 | typename MotionDerived::MotionPlain | ||
213 | ✗ | operator+(const MotionHelicalUnalignedTpl<S1, O1> & m1, const MotionDense<MotionDerived> & m2) | |
214 | { | ||
215 | ✗ | typename MotionDerived::MotionPlain res(m2); | |
216 | ✗ | res += m1; | |
217 | ✗ | return res; | |
218 | } | ||
219 | |||
220 | template<typename MotionDerived, typename S2, int O2> | ||
221 | EIGEN_STRONG_INLINE typename MotionDerived::MotionPlain | ||
222 | ✗ | operator^(const MotionDense<MotionDerived> & m1, const MotionHelicalUnalignedTpl<S2, O2> & m2) | |
223 | { | ||
224 | ✗ | return m2.motionAction(m1); | |
225 | } | ||
226 | |||
227 | template<typename Scalar, int Options> | ||
228 | struct JointMotionSubspaceHelicalUnalignedTpl; | ||
229 | |||
230 | template<typename Scalar, int Options> | ||
231 | struct SE3GroupAction<JointMotionSubspaceHelicalUnalignedTpl<Scalar, Options>> | ||
232 | { | ||
233 | typedef Eigen::Matrix<Scalar, 6, 1, Options> ReturnType; | ||
234 | }; | ||
235 | |||
236 | template<typename Scalar, int Options, typename MotionDerived> | ||
237 | struct MotionAlgebraAction<JointMotionSubspaceHelicalUnalignedTpl<Scalar, Options>, MotionDerived> | ||
238 | { | ||
239 | typedef Eigen::Matrix<Scalar, 6, 1, Options> ReturnType; | ||
240 | }; | ||
241 | |||
242 | template<typename Scalar, int Options, typename ForceDerived> | ||
243 | struct ConstraintForceOp<JointMotionSubspaceHelicalUnalignedTpl<Scalar, Options>, ForceDerived> | ||
244 | { | ||
245 | typedef typename Eigen::Matrix<Scalar, 1, 1> ReturnType; | ||
246 | }; | ||
247 | |||
248 | template<typename Scalar, int Options, typename ForceSet> | ||
249 | struct ConstraintForceSetOp<JointMotionSubspaceHelicalUnalignedTpl<Scalar, Options>, ForceSet> | ||
250 | { | ||
251 | typedef typename Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> ReturnType; | ||
252 | }; | ||
253 | |||
254 | template<typename _Scalar, int _Options> | ||
255 | struct traits<JointMotionSubspaceHelicalUnalignedTpl<_Scalar, _Options>> | ||
256 | { | ||
257 | typedef _Scalar Scalar; | ||
258 | enum | ||
259 | { | ||
260 | Options = _Options | ||
261 | }; | ||
262 | enum | ||
263 | { | ||
264 | LINEAR = 0, | ||
265 | ANGULAR = 3 | ||
266 | }; | ||
267 | |||
268 | typedef MotionHelicalUnalignedTpl<Scalar, Options> JointMotion; | ||
269 | typedef Eigen::Matrix<Scalar, 1, 1, Options> JointForce; | ||
270 | typedef Eigen::Matrix<Scalar, 6, 1, Options> DenseBase; | ||
271 | typedef Eigen::Matrix<Scalar, 1, 1, Options> ReducedSquaredMatrix; | ||
272 | |||
273 | typedef DenseBase MatrixReturnType; | ||
274 | typedef const DenseBase ConstMatrixReturnType; | ||
275 | |||
276 | typedef Eigen::Matrix<Scalar, 3, 1, Options> Vector3; | ||
277 | |||
278 | typedef typename ReducedSquaredMatrix::IdentityReturnType StDiagonalMatrixSOperationReturnType; | ||
279 | }; // traits JointMotionSubspaceHelicalUnalignedTpl | ||
280 | |||
281 | template<typename _Scalar, int _Options> | ||
282 | struct JointMotionSubspaceHelicalUnalignedTpl | ||
283 | : JointMotionSubspaceBase<JointMotionSubspaceHelicalUnalignedTpl<_Scalar, _Options>> | ||
284 | { | ||
285 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW | ||
286 | |||
287 | PINOCCHIO_CONSTRAINT_TYPEDEF_TPL(JointMotionSubspaceHelicalUnalignedTpl) | ||
288 | enum | ||
289 | { | ||
290 | NV = 1 | ||
291 | }; | ||
292 | |||
293 | ✗ | JointMotionSubspaceHelicalUnalignedTpl() | |
294 | { | ||
295 | } | ||
296 | |||
297 | typedef typename traits<JointMotionSubspaceHelicalUnalignedTpl>::Vector3 Vector3; | ||
298 | |||
299 | template<typename Vector3Like> | ||
300 | 23 | JointMotionSubspaceHelicalUnalignedTpl( | |
301 | const Eigen::MatrixBase<Vector3Like> & axis, const Scalar & h) | ||
302 | 23 | : m_axis(axis) | |
303 | 23 | , m_pitch(h) | |
304 | { | ||
305 | 23 | } | |
306 | |||
307 | template<typename Vector1Like> | ||
308 | 1 | JointMotion __mult__(const Eigen::MatrixBase<Vector1Like> & v) const | |
309 | { | ||
310 | EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector1Like, 1); | ||
311 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 1 times.
|
1 | assert(v.size() == 1); |
312 |
2/4✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
|
2 | return JointMotion(m_axis, v[0], Scalar(v[0] * m_pitch)); |
313 | } | ||
314 | |||
315 | template<typename S1, int O1> | ||
316 | typename SE3GroupAction<JointMotionSubspaceHelicalUnalignedTpl>::ReturnType | ||
317 | ✗ | se3Action(const SE3Tpl<S1, O1> & m) const | |
318 | { | ||
319 | typedef | ||
320 | typename SE3GroupAction<JointMotionSubspaceHelicalUnalignedTpl>::ReturnType ReturnType; | ||
321 | ✗ | ReturnType res; | |
322 | ✗ | res.template segment<3>(ANGULAR).noalias() = m.rotation() * m_axis; | |
323 | ✗ | res.template segment<3>(LINEAR).noalias() = | |
324 | ✗ | m.translation().cross(res.template segment<3>(ANGULAR)) + m_pitch * (m.rotation() * m_axis); | |
325 | ✗ | return res; | |
326 | } | ||
327 | |||
328 | template<typename S1, int O1> | ||
329 | typename SE3GroupAction<JointMotionSubspaceHelicalUnalignedTpl>::ReturnType | ||
330 | ✗ | se3ActionInverse(const SE3Tpl<S1, O1> & m) const | |
331 | { | ||
332 | typedef | ||
333 | typename SE3GroupAction<JointMotionSubspaceHelicalUnalignedTpl>::ReturnType ReturnType; | ||
334 | |||
335 | ✗ | ReturnType res; | |
336 | ✗ | res.template segment<3>(ANGULAR).noalias() = m.rotation().transpose() * m_axis; | |
337 | ✗ | res.template segment<3>(LINEAR).noalias() = | |
338 | ✗ | -m.rotation().transpose() * m.translation().cross(m_axis) | |
339 | ✗ | + m.rotation().transpose() * m_axis * m_pitch; | |
340 | |||
341 | ✗ | return res; | |
342 | } | ||
343 | |||
344 | ✗ | int nv_impl() const | |
345 | { | ||
346 | ✗ | return NV; | |
347 | } | ||
348 | |||
349 | struct TransposeConst : JointMotionSubspaceTransposeBase<JointMotionSubspaceHelicalUnalignedTpl> | ||
350 | { | ||
351 | const JointMotionSubspaceHelicalUnalignedTpl & ref; | ||
352 | ✗ | TransposeConst(const JointMotionSubspaceHelicalUnalignedTpl & ref) | |
353 | ✗ | : ref(ref) | |
354 | { | ||
355 | } | ||
356 | |||
357 | template<typename ForceDerived> | ||
358 | typename ConstraintForceOp<JointMotionSubspaceHelicalUnalignedTpl, ForceDerived>::ReturnType | ||
359 | ✗ | operator*(const ForceDense<ForceDerived> & f) const | |
360 | { | ||
361 | typedef typename ConstraintForceOp< | ||
362 | JointMotionSubspaceHelicalUnalignedTpl, ForceDerived>::ReturnType ReturnType; | ||
363 | ✗ | ReturnType res; | |
364 | ✗ | res[0] = ref.axis().dot(f.angular()) + ref.axis().dot(f.linear()) * ref.m_pitch; | |
365 | ✗ | return res; | |
366 | } | ||
367 | |||
368 | /// [CRBA] MatrixBase operator* (Constraint::Transpose S, ForceSet::Block) | ||
369 | template<typename Derived> | ||
370 | typename ConstraintForceSetOp<JointMotionSubspaceHelicalUnalignedTpl, Derived>::ReturnType | ||
371 | ✗ | operator*(const Eigen::MatrixBase<Derived> & F) const | |
372 | { | ||
373 | ✗ | assert(F.rows() == 6); | |
374 | return ( | ||
375 | ✗ | ref.axis().transpose() * F.template middleRows<3>(ANGULAR) | |
376 | ✗ | + (ref.axis().transpose() * F.template middleRows<3>(LINEAR) * ref.m_pitch)); | |
377 | } | ||
378 | }; // struct TransposeConst | ||
379 | |||
380 | ✗ | TransposeConst transpose() const | |
381 | { | ||
382 | ✗ | return TransposeConst(*this); | |
383 | } | ||
384 | |||
385 | /* CRBA joint operators | ||
386 | * - ForceSet::Block = ForceSet | ||
387 | * - ForceSet operator* (Inertia Y,Constraint S) | ||
388 | * - MatrixBase operator* (Constraint::Transpose S, ForceSet::Block) | ||
389 | * - SE3::act(ForceSet::Block) | ||
390 | */ | ||
391 | ✗ | DenseBase matrix_impl() const | |
392 | { | ||
393 | ✗ | DenseBase S; | |
394 | ✗ | S.template segment<3>(LINEAR) = m_axis * m_pitch; | |
395 | ✗ | S.template segment<3>(ANGULAR) = m_axis; | |
396 | ✗ | return S; | |
397 | } | ||
398 | |||
399 | template<typename MotionDerived> | ||
400 | typename MotionAlgebraAction<JointMotionSubspaceHelicalUnalignedTpl, MotionDerived>::ReturnType | ||
401 | ✗ | motionAction(const MotionDense<MotionDerived> & m) const | |
402 | { | ||
403 | ✗ | const typename MotionDerived::ConstLinearType v = m.linear(); | |
404 | ✗ | const typename MotionDerived::ConstAngularType w = m.angular(); | |
405 | |||
406 | ✗ | DenseBase res; | |
407 | ✗ | res.template segment<3>(LINEAR).noalias() = v.cross(m_axis); | |
408 | ✗ | res.template segment<3>(ANGULAR).noalias() = w.cross(m_axis * m_pitch); | |
409 | ✗ | res.template segment<3>(LINEAR).noalias() += res.template segment<3>(ANGULAR); | |
410 | ✗ | res.template segment<3>(ANGULAR).noalias() = w.cross(m_axis); | |
411 | |||
412 | ✗ | return res; | |
413 | } | ||
414 | |||
415 | ✗ | bool isEqual(const JointMotionSubspaceHelicalUnalignedTpl & other) const | |
416 | { | ||
417 | ✗ | return internal::comparison_eq(m_axis, other.m_axis) | |
418 | ✗ | && internal::comparison_eq(m_pitch, other.m_pitch); | |
419 | } | ||
420 | |||
421 | ✗ | Vector3 & axis() | |
422 | { | ||
423 | ✗ | return m_axis; | |
424 | } | ||
425 | ✗ | const Vector3 & axis() const | |
426 | { | ||
427 | ✗ | return m_axis; | |
428 | } | ||
429 | |||
430 | ✗ | Scalar & h() | |
431 | { | ||
432 | ✗ | return m_pitch; | |
433 | } | ||
434 | ✗ | const Scalar & h() const | |
435 | { | ||
436 | ✗ | return m_pitch; | |
437 | } | ||
438 | |||
439 | Vector3 m_axis; | ||
440 | Scalar m_pitch; | ||
441 | |||
442 | }; // struct JointMotionSubspaceHelicalUnalignedTpl | ||
443 | |||
444 | template<typename _Scalar, int _Options> | ||
445 | ✗ | Eigen::Matrix<_Scalar, 1, 1, _Options> operator*( | |
446 | const typename JointMotionSubspaceHelicalUnalignedTpl<_Scalar, _Options>::TransposeConst & | ||
447 | S_transpose, | ||
448 | const JointMotionSubspaceHelicalUnalignedTpl<_Scalar, _Options> & S) | ||
449 | { | ||
450 | ✗ | Eigen::Matrix<_Scalar, 1, 1, _Options> res; | |
451 | ✗ | res(0) = (S_transpose.ref.axis() * S_transpose.ref.h()).dot(S.axis() * S.h()) | |
452 | ✗ | + (S_transpose.ref.axis().dot(S.axis())); | |
453 | ✗ | return res; | |
454 | } | ||
455 | |||
456 | template<typename S1, int O1, typename S2, int O2> | ||
457 | struct MultiplicationOp<InertiaTpl<S1, O1>, JointMotionSubspaceHelicalUnalignedTpl<S2, O2>> | ||
458 | { | ||
459 | typedef Eigen::Matrix<S2, 6, 1, O2> ReturnType; | ||
460 | }; | ||
461 | |||
462 | /* [CRBA] ForceSet operator* (Inertia Y,Constraint S) */ | ||
463 | namespace impl | ||
464 | { | ||
465 | template<typename S1, int O1, typename S2, int O2> | ||
466 | struct LhsMultiplicationOp<InertiaTpl<S1, O1>, JointMotionSubspaceHelicalUnalignedTpl<S2, O2>> | ||
467 | { | ||
468 | typedef InertiaTpl<S1, O1> Inertia; | ||
469 | typedef JointMotionSubspaceHelicalUnalignedTpl<S2, O2> Constraint; | ||
470 | typedef typename JointMotionSubspaceHelicalUnalignedTpl<S2, O2>::Vector3 Vector3; | ||
471 | typedef typename MultiplicationOp<Inertia, Constraint>::ReturnType ReturnType; | ||
472 | ✗ | static inline ReturnType run(const Inertia & Y, const Constraint & cru) | |
473 | { | ||
474 | ✗ | ReturnType res; | |
475 | /* YS = [ m -mcx ; mcx I-mcxcx ] [ h ; w ] = [mh-mcxw ; mcxh+Iw-mcxcxw ] */ | ||
476 | |||
477 | ✗ | const S2 & m_pitch = cru.h(); | |
478 | ✗ | const typename Inertia::Scalar & m = Y.mass(); | |
479 | ✗ | const typename Inertia::Vector3 & c = Y.lever(); | |
480 | ✗ | const typename Inertia::Symmetric3 & I = Y.inertia(); | |
481 | |||
482 | ✗ | res.template segment<3>(Inertia::LINEAR) = -m * c.cross(cru.axis()); | |
483 | ✗ | res.template segment<3>(Inertia::ANGULAR).noalias() = I * cru.axis(); | |
484 | ✗ | res.template segment<3>(Inertia::ANGULAR) += c.cross(cru.axis()) * m * m_pitch; | |
485 | ✗ | res.template segment<3>(Inertia::ANGULAR) += | |
486 | ✗ | c.cross(res.template segment<3>(Inertia::LINEAR)); | |
487 | ✗ | res.template segment<3>(Inertia::LINEAR) += m * m_pitch * cru.axis(); | |
488 | ✗ | return res; | |
489 | } | ||
490 | }; | ||
491 | } // namespace impl | ||
492 | |||
493 | template<typename M6Like, typename Scalar, int Options> | ||
494 | struct MultiplicationOp< | ||
495 | Eigen::MatrixBase<M6Like>, | ||
496 | JointMotionSubspaceHelicalUnalignedTpl<Scalar, Options>> | ||
497 | { | ||
498 | typedef const Eigen::Matrix<Scalar, 6, 1> ReturnType; | ||
499 | }; | ||
500 | |||
501 | /* [ABA] operator* (Inertia Y,Constraint S) */ | ||
502 | namespace impl | ||
503 | { | ||
504 | template<typename M6Like, typename Scalar, int Options> | ||
505 | struct LhsMultiplicationOp< | ||
506 | Eigen::MatrixBase<M6Like>, | ||
507 | JointMotionSubspaceHelicalUnalignedTpl<Scalar, Options>> | ||
508 | { | ||
509 | typedef JointMotionSubspaceHelicalUnalignedTpl<Scalar, Options> Constraint; | ||
510 | typedef Eigen::Matrix<Scalar, 6, 1> ReturnType; | ||
511 | ✗ | static inline ReturnType run(const Eigen::MatrixBase<M6Like> & Y, const Constraint & cru) | |
512 | { | ||
513 | EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(M6Like, 6, 6); | ||
514 | ✗ | return Y.derived().template middleCols<3>(Constraint::ANGULAR) * cru.axis() | |
515 | ✗ | + Y.derived().template middleCols<3>(Constraint::LINEAR) * cru.axis() * cru.h(); | |
516 | } | ||
517 | }; | ||
518 | } // namespace impl | ||
519 | |||
520 | template<typename Scalar, int Options> | ||
521 | struct JointHelicalUnalignedTpl; | ||
522 | |||
523 | template<typename _Scalar, int _Options> | ||
524 | struct traits<JointHelicalUnalignedTpl<_Scalar, _Options>> | ||
525 | { | ||
526 | enum | ||
527 | { | ||
528 | NQ = 1, | ||
529 | NV = 1 | ||
530 | }; | ||
531 | typedef _Scalar Scalar; | ||
532 | enum | ||
533 | { | ||
534 | Options = _Options | ||
535 | }; | ||
536 | typedef JointDataHelicalUnalignedTpl<Scalar, Options> JointDataDerived; | ||
537 | typedef JointModelHelicalUnalignedTpl<Scalar, Options> JointModelDerived; | ||
538 | typedef JointMotionSubspaceHelicalUnalignedTpl<Scalar, Options> Constraint_t; | ||
539 | typedef SE3Tpl<Scalar, Options> Transformation_t; | ||
540 | typedef MotionHelicalUnalignedTpl<Scalar, Options> Motion_t; | ||
541 | typedef MotionZeroTpl<Scalar, Options> Bias_t; | ||
542 | |||
543 | // [ABA] | ||
544 | typedef Eigen::Matrix<Scalar, 6, NV, Options> U_t; | ||
545 | typedef Eigen::Matrix<Scalar, NV, NV, Options> D_t; | ||
546 | typedef Eigen::Matrix<Scalar, 6, NV, Options> UD_t; | ||
547 | |||
548 | typedef Eigen::Matrix<Scalar, NQ, 1, Options> ConfigVector_t; | ||
549 | typedef Eigen::Matrix<Scalar, NV, 1, Options> TangentVector_t; | ||
550 | |||
551 | PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE | ||
552 | }; | ||
553 | |||
554 | template<typename _Scalar, int _Options> | ||
555 | struct traits<JointDataHelicalUnalignedTpl<_Scalar, _Options>> | ||
556 | { | ||
557 | typedef JointHelicalUnalignedTpl<_Scalar, _Options> JointDerived; | ||
558 | typedef _Scalar Scalar; | ||
559 | }; | ||
560 | |||
561 | template<typename _Scalar, int _Options> | ||
562 | struct traits<JointModelHelicalUnalignedTpl<_Scalar, _Options>> | ||
563 | { | ||
564 | typedef JointHelicalUnalignedTpl<_Scalar, _Options> JointDerived; | ||
565 | typedef _Scalar Scalar; | ||
566 | }; | ||
567 | |||
568 | template<typename _Scalar, int _Options> | ||
569 | struct JointDataHelicalUnalignedTpl | ||
570 | : public JointDataBase<JointDataHelicalUnalignedTpl<_Scalar, _Options>> | ||
571 | { | ||
572 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW | ||
573 | typedef JointHelicalUnalignedTpl<_Scalar, _Options> JointDerived; | ||
574 | PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(JointDerived); | ||
575 | 1 | PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR | |
576 | |||
577 | ConfigVector_t joint_q; | ||
578 | TangentVector_t joint_v; | ||
579 | |||
580 | Constraint_t S; | ||
581 | Transformation_t M; | ||
582 | Motion_t v; | ||
583 | Bias_t c; | ||
584 | |||
585 | // [ABA] specific data | ||
586 | U_t U; | ||
587 | D_t Dinv; | ||
588 | UD_t UDinv; | ||
589 | D_t StU; | ||
590 | |||
591 | 23 | JointDataHelicalUnalignedTpl() | |
592 |
1/2✓ Branch 2 taken 23 times.
✗ Branch 3 not taken.
|
23 | : joint_q(ConfigVector_t::Zero()) |
593 |
1/2✓ Branch 2 taken 23 times.
✗ Branch 3 not taken.
|
23 | , joint_v(TangentVector_t::Zero()) |
594 |
2/4✓ Branch 1 taken 23 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 23 times.
✗ Branch 5 not taken.
|
23 | , S(Constraint_t::Vector3::Zero(), (Scalar)0) |
595 | 23 | , M(Transformation_t::Identity()) | |
596 |
2/4✓ Branch 1 taken 23 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 23 times.
✗ Branch 5 not taken.
|
23 | , v(Constraint_t::Vector3::Zero(), (Scalar)0, (Scalar)0) |
597 |
1/2✓ Branch 2 taken 23 times.
✗ Branch 3 not taken.
|
23 | , U(U_t::Zero()) |
598 |
1/2✓ Branch 2 taken 23 times.
✗ Branch 3 not taken.
|
23 | , Dinv(D_t::Zero()) |
599 |
1/2✓ Branch 2 taken 23 times.
✗ Branch 3 not taken.
|
23 | , UDinv(UD_t::Zero()) |
600 |
1/2✓ Branch 3 taken 23 times.
✗ Branch 4 not taken.
|
46 | , StU(D_t::Zero()) |
601 | { | ||
602 | 23 | } | |
603 | |||
604 | template<typename Vector3Like> | ||
605 | ✗ | JointDataHelicalUnalignedTpl(const Eigen::MatrixBase<Vector3Like> & axis) | |
606 | ✗ | : joint_q(ConfigVector_t::Zero()) | |
607 | ✗ | , joint_v(TangentVector_t::Zero()) | |
608 | ✗ | , S(axis, (Scalar)0) | |
609 | ✗ | , M(Transformation_t::Identity()) | |
610 | ✗ | , v(axis, (Scalar)0, (Scalar)0) | |
611 | ✗ | , U(U_t::Zero()) | |
612 | ✗ | , Dinv(D_t::Zero()) | |
613 | ✗ | , UDinv(UD_t::Zero()) | |
614 | ✗ | , StU(D_t::Zero()) | |
615 | { | ||
616 | } | ||
617 | |||
618 | 40 | static std::string classname() | |
619 | { | ||
620 |
1/2✓ Branch 2 taken 40 times.
✗ Branch 3 not taken.
|
40 | return std::string("JointDataHelicalUnaligned"); |
621 | } | ||
622 | ✗ | std::string shortname() const | |
623 | { | ||
624 | ✗ | return classname(); | |
625 | } | ||
626 | |||
627 | }; // struct JointDataHelicalUnalignedTpl | ||
628 | |||
629 | PINOCCHIO_JOINT_CAST_TYPE_SPECIALIZATION(JointModelHelicalUnalignedTpl); | ||
630 | template<typename _Scalar, int _Options> | ||
631 | struct JointModelHelicalUnalignedTpl | ||
632 | : public JointModelBase<JointModelHelicalUnalignedTpl<_Scalar, _Options>> | ||
633 | { | ||
634 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW | ||
635 | typedef JointHelicalUnalignedTpl<_Scalar, _Options> JointDerived; | ||
636 | PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived); | ||
637 | typedef Eigen::Matrix<Scalar, 3, 1, _Options> Vector3; | ||
638 | |||
639 | typedef JointModelBase<JointModelHelicalUnalignedTpl> Base; | ||
640 | using Base::id; | ||
641 | using Base::idx_q; | ||
642 | using Base::idx_v; | ||
643 | using Base::setIndexes; | ||
644 | |||
645 | 20 | JointModelHelicalUnalignedTpl() | |
646 | 20 | { | |
647 | 20 | } | |
648 | |||
649 | template<typename Vector3Like> | ||
650 | ✗ | JointModelHelicalUnalignedTpl(const Eigen::MatrixBase<Vector3Like> & axis) | |
651 | ✗ | : axis(axis) | |
652 | ✗ | , m_pitch((Scalar)0) | |
653 | { | ||
654 | EIGEN_STATIC_ASSERT_VECTOR_ONLY(Vector3Like); | ||
655 | ✗ | assert(isUnitary(axis) && "Rotation axis is not unitary"); | |
656 | } | ||
657 | |||
658 | ✗ | JointModelHelicalUnalignedTpl( | |
659 | const Scalar & x, const Scalar & y, const Scalar & z, const Scalar & h) | ||
660 | ✗ | : axis(x, y, z) | |
661 | ✗ | , m_pitch(h) | |
662 | { | ||
663 | ✗ | normalize(axis); | |
664 | ✗ | assert(isUnitary(axis) && "Rotation axis is not unitary"); | |
665 | } | ||
666 | |||
667 | template<typename Vector3Like> | ||
668 | ✗ | JointModelHelicalUnalignedTpl(const Eigen::MatrixBase<Vector3Like> & axis, const Scalar & h) | |
669 | ✗ | : axis(axis) | |
670 | ✗ | , m_pitch(h) | |
671 | { | ||
672 | EIGEN_STATIC_ASSERT_VECTOR_ONLY(Vector3Like); | ||
673 | ✗ | assert(isUnitary(axis) && "Rotation axis is not unitary"); | |
674 | } | ||
675 | |||
676 | ✗ | const std::vector<bool> hasConfigurationLimit() const | |
677 | { | ||
678 | ✗ | return {true, true}; | |
679 | } | ||
680 | |||
681 | ✗ | const std::vector<bool> hasConfigurationLimitInTangent() const | |
682 | { | ||
683 | ✗ | return {true, true}; | |
684 | } | ||
685 | |||
686 | 3 | JointDataDerived createData() const | |
687 | { | ||
688 | 3 | return JointDataDerived(); | |
689 | } | ||
690 | |||
691 | using Base::isEqual; | ||
692 | ✗ | bool isEqual(const JointModelHelicalUnalignedTpl & other) const | |
693 | { | ||
694 | ✗ | return Base::isEqual(other) && internal::comparison_eq(axis, other.axis) | |
695 | ✗ | && internal::comparison_eq(m_pitch, other.m_pitch); | |
696 | } | ||
697 | |||
698 | template<typename ConfigVector> | ||
699 | ✗ | void calc(JointDataDerived & data, const typename Eigen::MatrixBase<ConfigVector> & qs) const | |
700 | { | ||
701 | ✗ | data.joint_q[0] = qs[idx_q()]; | |
702 | |||
703 | ✗ | toRotationMatrix(axis, data.joint_q[0], data.M.rotation()); | |
704 | ✗ | data.M.translation() = axis * data.joint_q[0] * m_pitch; | |
705 | ✗ | data.S.h() = m_pitch; | |
706 | ✗ | data.S.axis() = axis; | |
707 | } | ||
708 | |||
709 | template<typename TangentVector> | ||
710 | void | ||
711 | ✗ | calc(JointDataDerived & data, const Blank, const typename Eigen::MatrixBase<TangentVector> & vs) | |
712 | const | ||
713 | { | ||
714 | ✗ | data.v.angularRate() = static_cast<Scalar>(vs[idx_v()]); | |
715 | ✗ | data.v.axis() = axis; | |
716 | ✗ | data.v.linearRate() = static_cast<Scalar>(vs[idx_v()] * m_pitch); | |
717 | } | ||
718 | |||
719 | template<typename ConfigVector, typename TangentVector> | ||
720 | 3 | void calc( | |
721 | JointDataDerived & data, | ||
722 | const typename Eigen::MatrixBase<ConfigVector> & qs, | ||
723 | const typename Eigen::MatrixBase<TangentVector> & vs) const | ||
724 | { | ||
725 | 3 | calc(data, qs.derived()); | |
726 | 3 | data.v.angularRate() = static_cast<Scalar>(vs[idx_v()]); | |
727 | 3 | data.v.axis() = axis; | |
728 | 3 | data.v.linearRate() = static_cast<Scalar>(vs[idx_v()] * m_pitch); | |
729 | 3 | } | |
730 | |||
731 | template<typename VectorLike, typename Matrix6Like> | ||
732 | ✗ | void calc_aba( | |
733 | JointDataDerived & data, | ||
734 | const Eigen::MatrixBase<VectorLike> & armature, | ||
735 | const Eigen::MatrixBase<Matrix6Like> & I, | ||
736 | const bool update_I) const | ||
737 | { | ||
738 | ✗ | data.U.noalias() = I.template middleCols<3>(Motion::ANGULAR) * axis | |
739 | ✗ | + m_pitch * I.template middleCols<3>(Motion::LINEAR) * axis; | |
740 | ✗ | data.StU[0] = axis.dot(data.U.template segment<3>(Motion::ANGULAR)) | |
741 | ✗ | + m_pitch * axis.dot(data.U.template segment<3>(Motion::LINEAR)) + armature[0]; | |
742 | ✗ | data.Dinv[0] = Scalar(1) / data.StU[0]; | |
743 | ✗ | data.UDinv.noalias() = data.U * data.Dinv; | |
744 | ✗ | if (update_I) | |
745 | ✗ | PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like, I).noalias() -= data.UDinv * data.U.transpose(); | |
746 | } | ||
747 | |||
748 | 40 | static std::string classname() | |
749 | { | ||
750 |
1/2✓ Branch 2 taken 40 times.
✗ Branch 3 not taken.
|
40 | return std::string("JointModelHelicalUnaligned"); |
751 | } | ||
752 | ✗ | std::string shortname() const | |
753 | { | ||
754 | ✗ | return classname(); | |
755 | } | ||
756 | |||
757 | /// \returns An expression of *this with the Scalar type casted to NewScalar. | ||
758 | template<typename NewScalar> | ||
759 | ✗ | JointModelHelicalUnalignedTpl<NewScalar, Options> cast() const | |
760 | { | ||
761 | typedef JointModelHelicalUnalignedTpl<NewScalar, Options> ReturnType; | ||
762 | ✗ | ReturnType res(axis.template cast<NewScalar>(), ScalarCast<NewScalar, Scalar>::cast(m_pitch)); | |
763 | ✗ | res.setIndexes(id(), idx_q(), idx_v()); | |
764 | ✗ | return res; | |
765 | } | ||
766 | |||
767 | Vector3 axis; | ||
768 | Scalar m_pitch; | ||
769 | |||
770 | }; // struct JointModelHelicalUnalignedTpl | ||
771 | |||
772 | } // namespace pinocchio | ||
773 | |||
774 | #include <boost/type_traits.hpp> | ||
775 | |||
776 | namespace boost | ||
777 | { | ||
778 | template<typename Scalar, int Options> | ||
779 | struct has_nothrow_constructor<::pinocchio::JointModelHelicalUnalignedTpl<Scalar, Options>> | ||
780 | : public integral_constant<bool, true> | ||
781 | { | ||
782 | }; | ||
783 | |||
784 | template<typename Scalar, int Options> | ||
785 | struct has_nothrow_copy<::pinocchio::JointModelHelicalUnalignedTpl<Scalar, Options>> | ||
786 | : public integral_constant<bool, true> | ||
787 | { | ||
788 | }; | ||
789 | |||
790 | template<typename Scalar, int Options> | ||
791 | struct has_nothrow_constructor<::pinocchio::JointDataHelicalUnalignedTpl<Scalar, Options>> | ||
792 | : public integral_constant<bool, true> | ||
793 | { | ||
794 | }; | ||
795 | |||
796 | template<typename Scalar, int Options> | ||
797 | struct has_nothrow_copy<::pinocchio::JointDataHelicalUnalignedTpl<Scalar, Options>> | ||
798 | : public integral_constant<bool, true> | ||
799 | { | ||
800 | }; | ||
801 | } // namespace boost | ||
802 | |||
803 | #endif // ifndef __pinocchio_multibody_joint_helical_unaligned_hpp__ | ||
804 |