Directory: | ./ |
---|---|
File: | include/pinocchio/multibody/joint/joint-helical.hpp |
Date: | 2024-08-27 18:20:05 |
Exec | Total | Coverage | |
---|---|---|---|
Lines: | 41 | 243 | 16.9% |
Branches: | 12 | 352 | 3.4% |
Line | Branch | Exec | Source |
---|---|---|---|
1 | // | ||
2 | // Copyright (c) 2022-2023 INRIA | ||
3 | // | ||
4 | |||
5 | #ifndef __pinocchio_multibody_joint_helical_hpp__ | ||
6 | #define __pinocchio_multibody_joint_helical_hpp__ | ||
7 | |||
8 | #include "pinocchio/math/sincos.hpp" | ||
9 | #include "pinocchio/spatial/inertia.hpp" | ||
10 | #include "pinocchio/multibody/joint-motion-subspace.hpp" | ||
11 | #include "pinocchio/multibody/joint/joint-base.hpp" | ||
12 | #include "pinocchio/spatial/spatial-axis.hpp" | ||
13 | #include "pinocchio/utils/axis-label.hpp" | ||
14 | |||
15 | namespace pinocchio | ||
16 | { | ||
17 | |||
18 | template<typename Scalar, int Options, int axis> | ||
19 | struct MotionHelicalTpl; | ||
20 | |||
21 | template<typename Scalar, int Options, int axis> | ||
22 | struct SE3GroupAction<MotionHelicalTpl<Scalar, Options, axis>> | ||
23 | { | ||
24 | typedef MotionTpl<Scalar, Options> ReturnType; | ||
25 | }; | ||
26 | |||
27 | template<typename Scalar, int Options, int axis, typename MotionDerived> | ||
28 | struct MotionAlgebraAction<MotionHelicalTpl<Scalar, Options, axis>, MotionDerived> | ||
29 | { | ||
30 | typedef MotionTpl<Scalar, Options> ReturnType; | ||
31 | }; | ||
32 | |||
33 | template<typename _Scalar, int _Options, int axis> | ||
34 | struct traits<MotionHelicalTpl<_Scalar, _Options, axis>> | ||
35 | { | ||
36 | typedef _Scalar Scalar; | ||
37 | enum | ||
38 | { | ||
39 | Options = _Options | ||
40 | }; | ||
41 | typedef Eigen::Matrix<Scalar, 3, 1, Options> Vector3; | ||
42 | typedef Eigen::Matrix<Scalar, 6, 1, Options> Vector6; | ||
43 | typedef Eigen::Matrix<Scalar, 4, 4, Options> Matrix4; | ||
44 | typedef Eigen::Matrix<Scalar, 6, 6, Options> Matrix6; | ||
45 | typedef typename PINOCCHIO_EIGEN_REF_CONST_TYPE(Vector6) ToVectorConstReturnType; | ||
46 | typedef typename PINOCCHIO_EIGEN_REF_TYPE(Vector6) ToVectorReturnType; | ||
47 | typedef Vector3 AngularType; | ||
48 | typedef Vector3 LinearType; | ||
49 | typedef const Vector3 ConstAngularType; | ||
50 | typedef const Vector3 ConstLinearType; | ||
51 | typedef Matrix6 ActionMatrixType; | ||
52 | typedef MotionTpl<Scalar, Options> MotionPlain; | ||
53 | typedef MotionPlain PlainReturnType; | ||
54 | typedef Matrix4 HomogeneousMatrixType; | ||
55 | enum | ||
56 | { | ||
57 | LINEAR = 0, | ||
58 | ANGULAR = 3 | ||
59 | }; | ||
60 | }; // traits MotionHelicalTpl | ||
61 | |||
62 | template<typename Scalar, int Options, int axis> | ||
63 | struct TransformHelicalTpl; | ||
64 | |||
65 | template<typename _Scalar, int _Options, int _axis> | ||
66 | struct traits<TransformHelicalTpl<_Scalar, _Options, _axis>> | ||
67 | { | ||
68 | enum | ||
69 | { | ||
70 | axis = _axis, | ||
71 | Options = _Options, | ||
72 | LINEAR = 0, | ||
73 | ANGULAR = 3 | ||
74 | }; | ||
75 | typedef _Scalar Scalar; | ||
76 | typedef SE3Tpl<Scalar, Options> PlainType; | ||
77 | typedef Eigen::Matrix<Scalar, 3, 1, Options> Vector3; | ||
78 | typedef Eigen::Matrix<Scalar, 3, 3, Options> Matrix3; | ||
79 | typedef Matrix3 AngularType; | ||
80 | typedef Matrix3 AngularRef; | ||
81 | typedef Matrix3 ConstAngularRef; | ||
82 | typedef Vector3 LinearType; | ||
83 | typedef typename Vector3::ConstantReturnType LinearRef; | ||
84 | typedef const typename Vector3::ConstantReturnType ConstLinearRef; | ||
85 | typedef typename traits<PlainType>::ActionMatrixType ActionMatrixType; | ||
86 | typedef typename traits<PlainType>::HomogeneousMatrixType HomogeneousMatrixType; | ||
87 | }; // traits TransformHelicalTpl | ||
88 | |||
89 | template<typename Scalar, int Options, int axis> | ||
90 | struct SE3GroupAction<TransformHelicalTpl<Scalar, Options, axis>> | ||
91 | { | ||
92 | typedef typename traits<TransformHelicalTpl<Scalar, Options, axis>>::PlainType ReturnType; | ||
93 | }; | ||
94 | |||
95 | template<typename _Scalar, int _Options, int axis> | ||
96 | struct TransformHelicalTpl : SE3Base<TransformHelicalTpl<_Scalar, _Options, axis>> | ||
97 | { | ||
98 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW | ||
99 | PINOCCHIO_SE3_TYPEDEF_TPL(TransformHelicalTpl); | ||
100 | |||
101 | typedef SpatialAxis<axis + LINEAR> AxisLinear; | ||
102 | typedef typename AxisLinear::CartesianAxis3 CartesianAxis3Linear; | ||
103 | |||
104 | ✗ | TransformHelicalTpl() | |
105 | { | ||
106 | } | ||
107 | 132 | TransformHelicalTpl(const Scalar & sin, const Scalar & cos, const Scalar & displacement) | |
108 | 132 | : m_sin(sin) | |
109 | 132 | , m_cos(cos) | |
110 | 132 | , m_displacement(displacement) | |
111 | { | ||
112 | 132 | } | |
113 | |||
114 | ✗ | PlainType plain() const | |
115 | { | ||
116 | ✗ | PlainType res(PlainType::Identity()); | |
117 | ✗ | _setRotation(res.rotation()); | |
118 | ✗ | res.translation()[axis] = m_displacement; | |
119 | ✗ | return res; | |
120 | } | ||
121 | |||
122 | ✗ | operator PlainType() const | |
123 | { | ||
124 | ✗ | return plain(); | |
125 | } | ||
126 | |||
127 | template<typename S2, int O2> | ||
128 | typename SE3GroupAction<TransformHelicalTpl>::ReturnType | ||
129 | se3action(const SE3Tpl<S2, O2> & m) const | ||
130 | { | ||
131 | typedef typename SE3GroupAction<TransformHelicalTpl>::ReturnType ReturnType; | ||
132 | ReturnType res; | ||
133 | switch (axis) | ||
134 | { | ||
135 | case 0: { | ||
136 | res.rotation().col(0) = m.rotation().col(0); | ||
137 | res.rotation().col(1).noalias() = m_cos * m.rotation().col(1) + m_sin * m.rotation().col(2); | ||
138 | res.rotation().col(2).noalias() = res.rotation().col(0).cross(res.rotation().col(1)); | ||
139 | break; | ||
140 | } | ||
141 | case 1: { | ||
142 | res.rotation().col(2).noalias() = m_cos * m.rotation().col(2) + m_sin * m.rotation().col(0); | ||
143 | res.rotation().col(1) = m.rotation().col(1); | ||
144 | res.rotation().col(0).noalias() = res.rotation().col(1).cross(res.rotation().col(2)); | ||
145 | break; | ||
146 | } | ||
147 | case 2: { | ||
148 | res.rotation().col(0).noalias() = m_cos * m.rotation().col(0) + m_sin * m.rotation().col(1); | ||
149 | res.rotation().col(1).noalias() = res.rotation().col(2).cross(res.rotation().col(0)); | ||
150 | res.rotation().col(2) = m.rotation().col(2); | ||
151 | break; | ||
152 | } | ||
153 | default: { | ||
154 | assert(false && "must never happen"); | ||
155 | break; | ||
156 | } | ||
157 | } | ||
158 | res.translation() = m.translation(); | ||
159 | res.translation()[axis] += m_displacement; | ||
160 | return res; | ||
161 | } | ||
162 | |||
163 | const Scalar & sin() const | ||
164 | { | ||
165 | return m_sin; | ||
166 | } | ||
167 | ✗ | Scalar & sin() | |
168 | { | ||
169 | ✗ | return m_sin; | |
170 | } | ||
171 | |||
172 | const Scalar & cos() const | ||
173 | { | ||
174 | return m_cos; | ||
175 | } | ||
176 | ✗ | Scalar & cos() | |
177 | { | ||
178 | ✗ | return m_cos; | |
179 | } | ||
180 | |||
181 | ✗ | const Scalar & displacement() const | |
182 | { | ||
183 | ✗ | return m_displacement; | |
184 | } | ||
185 | ✗ | Scalar & displacement() | |
186 | { | ||
187 | ✗ | return m_displacement; | |
188 | } | ||
189 | |||
190 | template<typename Scalar1, typename Scalar2, typename Scalar3> | ||
191 | ✗ | void setValues(const Scalar1 & sin, const Scalar2 & cos, const Scalar3 & displacement) | |
192 | { | ||
193 | ✗ | m_sin = sin; | |
194 | ✗ | m_cos = cos; | |
195 | ✗ | m_displacement = displacement; | |
196 | } | ||
197 | |||
198 | ✗ | LinearType translation() const | |
199 | { | ||
200 | ✗ | return CartesianAxis3Linear() * displacement(); | |
201 | } | ||
202 | ✗ | AngularType rotation() const | |
203 | { | ||
204 | ✗ | AngularType m(AngularType::Identity()); | |
205 | ✗ | _setRotation(m); | |
206 | ✗ | return m; | |
207 | } | ||
208 | |||
209 | ✗ | bool isEqual(const TransformHelicalTpl & other) const | |
210 | { | ||
211 | ✗ | return internal::comparison_eq(m_cos, other.m_cos) | |
212 | ✗ | && internal::comparison_eq(m_sin, other.m_sin) | |
213 | ✗ | && internal::comparison_eq(m_displacement, other.m_displacement); | |
214 | } | ||
215 | |||
216 | protected: | ||
217 | Scalar m_sin, m_cos, m_displacement; | ||
218 | ✗ | inline void _setRotation(typename PlainType::AngularRef & rot) const | |
219 | { | ||
220 | switch (axis) | ||
221 | { | ||
222 | case 0: { | ||
223 | ✗ | rot.coeffRef(1, 1) = m_cos; | |
224 | ✗ | rot.coeffRef(1, 2) = -m_sin; | |
225 | ✗ | rot.coeffRef(2, 1) = m_sin; | |
226 | ✗ | rot.coeffRef(2, 2) = m_cos; | |
227 | ✗ | break; | |
228 | } | ||
229 | case 1: { | ||
230 | ✗ | rot.coeffRef(0, 0) = m_cos; | |
231 | ✗ | rot.coeffRef(0, 2) = m_sin; | |
232 | ✗ | rot.coeffRef(2, 0) = -m_sin; | |
233 | ✗ | rot.coeffRef(2, 2) = m_cos; | |
234 | ✗ | break; | |
235 | } | ||
236 | case 2: { | ||
237 | ✗ | rot.coeffRef(0, 0) = m_cos; | |
238 | ✗ | rot.coeffRef(0, 1) = -m_sin; | |
239 | ✗ | rot.coeffRef(1, 0) = m_sin; | |
240 | ✗ | rot.coeffRef(1, 1) = m_cos; | |
241 | ✗ | break; | |
242 | } | ||
243 | default: { | ||
244 | assert(false && "must never happen"); | ||
245 | break; | ||
246 | } | ||
247 | } | ||
248 | } | ||
249 | }; // struct TransformHelicalTpl | ||
250 | |||
251 | template<typename _Scalar, int _Options, int axis> | ||
252 | struct MotionHelicalTpl : MotionBase<MotionHelicalTpl<_Scalar, _Options, axis>> | ||
253 | { | ||
254 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW | ||
255 | |||
256 | MOTION_TYPEDEF_TPL(MotionHelicalTpl); | ||
257 | typedef SpatialAxis<axis + ANGULAR> AxisAngular; | ||
258 | typedef typename AxisAngular::CartesianAxis3 CartesianAxis3Angular; | ||
259 | typedef SpatialAxis<axis + LINEAR> AxisLinear; | ||
260 | typedef typename AxisLinear::CartesianAxis3 CartesianAxis3Linear; | ||
261 | |||
262 | ✗ | MotionHelicalTpl() | |
263 | { | ||
264 | } | ||
265 | |||
266 | 132 | MotionHelicalTpl(const Scalar & w, const Scalar & v) | |
267 | 132 | : m_w(w) | |
268 | 132 | , m_v(v) | |
269 | { | ||
270 | 132 | } | |
271 | |||
272 | ✗ | inline PlainReturnType plain() const | |
273 | { | ||
274 | ✗ | return PlainReturnType(CartesianAxis3Linear() * m_v, CartesianAxis3Angular() * m_w); | |
275 | } | ||
276 | |||
277 | template<typename OtherScalar> | ||
278 | MotionHelicalTpl __mult__(const OtherScalar & alpha) const | ||
279 | { | ||
280 | return MotionHelicalTpl(alpha * m_w, alpha * m_v); | ||
281 | } | ||
282 | |||
283 | template<typename MotionDerived> | ||
284 | ✗ | void setTo(MotionDense<MotionDerived> & m) const | |
285 | { | ||
286 | ✗ | for (Eigen::DenseIndex k = 0; k < 3; ++k) | |
287 | { | ||
288 | ✗ | m.angular()[k] = k == axis ? m_w : (Scalar)0; | |
289 | ✗ | m.linear()[k] = k == axis ? m_v : (Scalar)0; | |
290 | } | ||
291 | } | ||
292 | |||
293 | template<typename MotionDerived> | ||
294 | ✗ | inline void addTo(MotionDense<MotionDerived> & v) const | |
295 | { | ||
296 | typedef typename MotionDense<MotionDerived>::Scalar OtherScalar; | ||
297 | ✗ | v.angular()[axis] += (OtherScalar)m_w; | |
298 | ✗ | v.linear()[axis] += (OtherScalar)m_v; | |
299 | } | ||
300 | |||
301 | template<typename S2, int O2, typename D2> | ||
302 | ✗ | inline void se3Action_impl(const SE3Tpl<S2, O2> & m, MotionDense<D2> & v) const | |
303 | { | ||
304 | ✗ | v.angular().noalias() = m.rotation().col(axis) * m_w; | |
305 | ✗ | v.linear().noalias() = m.translation().cross(v.angular()) + m_v * (m.rotation().col(axis)); | |
306 | } | ||
307 | |||
308 | template<typename S2, int O2> | ||
309 | ✗ | MotionPlain se3Action_impl(const SE3Tpl<S2, O2> & m) const | |
310 | { | ||
311 | |||
312 | ✗ | MotionPlain res; | |
313 | ✗ | se3Action_impl(m, res); | |
314 | ✗ | return res; | |
315 | } | ||
316 | |||
317 | template<typename S2, int O2, typename D2> | ||
318 | ✗ | void se3ActionInverse_impl(const SE3Tpl<S2, O2> & m, MotionDense<D2> & v) const | |
319 | { | ||
320 | // Linear | ||
321 | ✗ | CartesianAxis3Linear::alphaCross(m_w, m.translation(), v.angular()); | |
322 | ✗ | v.linear().noalias() = | |
323 | ✗ | m.rotation().transpose() * v.angular() + m_v * (m.rotation().transpose().col(axis)); | |
324 | |||
325 | // Angular | ||
326 | ✗ | v.angular().noalias() = m.rotation().transpose().col(axis) * m_w; | |
327 | } | ||
328 | |||
329 | template<typename S2, int O2> | ||
330 | ✗ | MotionPlain se3ActionInverse_impl(const SE3Tpl<S2, O2> & m) const | |
331 | { | ||
332 | ✗ | MotionPlain res; | |
333 | ✗ | se3ActionInverse_impl(m, res); | |
334 | ✗ | return res; | |
335 | } | ||
336 | |||
337 | template<typename M1, typename M2> | ||
338 | ✗ | EIGEN_STRONG_INLINE void motionAction(const MotionDense<M1> & v, MotionDense<M2> & mout) const | |
339 | { | ||
340 | // Linear | ||
341 | ✗ | CartesianAxis3Linear::alphaCross(-m_w, v.linear(), mout.linear()); | |
342 | ✗ | CartesianAxis3Linear::alphaCross(-m_v, v.angular(), mout.angular()); | |
343 | ✗ | mout.linear() += mout.angular(); | |
344 | // Angular | ||
345 | ✗ | CartesianAxis3Angular::alphaCross(-m_w, v.angular(), mout.angular()); | |
346 | } | ||
347 | |||
348 | template<typename M1> | ||
349 | ✗ | MotionPlain motionAction(const MotionDense<M1> & v) const | |
350 | { | ||
351 | ✗ | MotionPlain res; | |
352 | ✗ | motionAction(v, res); | |
353 | ✗ | return res; | |
354 | } | ||
355 | |||
356 | ✗ | Scalar & angularRate() | |
357 | { | ||
358 | ✗ | return m_w; | |
359 | } | ||
360 | const Scalar & angularRate() const | ||
361 | { | ||
362 | return m_w; | ||
363 | } | ||
364 | |||
365 | ✗ | Scalar & linearRate() | |
366 | { | ||
367 | ✗ | return m_v; | |
368 | } | ||
369 | const Scalar & linearRate() const | ||
370 | { | ||
371 | return m_v; | ||
372 | } | ||
373 | |||
374 | ✗ | bool isEqual_impl(const MotionHelicalTpl & other) const | |
375 | { | ||
376 | ✗ | return internal::comparison_eq(m_w, other.m_w) && internal::comparison_eq(m_v, other.m_v); | |
377 | } | ||
378 | |||
379 | protected: | ||
380 | Scalar m_w, m_v; | ||
381 | }; // struct MotionHelicalTpl | ||
382 | template<typename S1, int O1, int axis, typename MotionDerived> | ||
383 | typename MotionDerived::MotionPlain | ||
384 | ✗ | operator+(const MotionHelicalTpl<S1, O1, axis> & m1, const MotionDense<MotionDerived> & m2) | |
385 | { | ||
386 | ✗ | typename MotionDerived::MotionPlain res(m2); | |
387 | ✗ | res += m1; | |
388 | ✗ | return res; | |
389 | } | ||
390 | |||
391 | template<typename MotionDerived, typename S2, int O2, int axis> | ||
392 | EIGEN_STRONG_INLINE typename MotionDerived::MotionPlain | ||
393 | ✗ | operator^(const MotionDense<MotionDerived> & m1, const MotionHelicalTpl<S2, O2, axis> & m2) | |
394 | { | ||
395 | ✗ | return m2.motionAction(m1); | |
396 | } | ||
397 | |||
398 | template<typename Scalar, int Options, int axis> | ||
399 | struct JointMotionSubspaceHelicalTpl; | ||
400 | |||
401 | template<typename Scalar, int Options, int axis> | ||
402 | struct SE3GroupAction<JointMotionSubspaceHelicalTpl<Scalar, Options, axis>> | ||
403 | { | ||
404 | typedef Eigen::Matrix<Scalar, 6, 1, Options> ReturnType; | ||
405 | }; | ||
406 | |||
407 | template<typename Scalar, int Options, int axis, typename MotionDerived> | ||
408 | struct MotionAlgebraAction<JointMotionSubspaceHelicalTpl<Scalar, Options, axis>, MotionDerived> | ||
409 | { | ||
410 | typedef Eigen::Matrix<Scalar, 6, 1, Options> ReturnType; | ||
411 | }; | ||
412 | |||
413 | template<typename Scalar, int Options, int axis, typename ForceDerived> | ||
414 | struct ConstraintForceOp<JointMotionSubspaceHelicalTpl<Scalar, Options, axis>, ForceDerived> | ||
415 | { | ||
416 | typedef typename Eigen::Matrix<Scalar, 1, 1> ReturnType; | ||
417 | }; | ||
418 | |||
419 | template<typename Scalar, int Options, int axis, typename ForceSet> | ||
420 | struct ConstraintForceSetOp<JointMotionSubspaceHelicalTpl<Scalar, Options, axis>, ForceSet> | ||
421 | { | ||
422 | typedef typename Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> ReturnType; | ||
423 | }; | ||
424 | |||
425 | template<typename _Scalar, int _Options, int axis> | ||
426 | struct traits<JointMotionSubspaceHelicalTpl<_Scalar, _Options, axis>> | ||
427 | { | ||
428 | typedef _Scalar Scalar; | ||
429 | enum | ||
430 | { | ||
431 | Options = _Options | ||
432 | }; | ||
433 | enum | ||
434 | { | ||
435 | LINEAR = 0, | ||
436 | ANGULAR = 3 | ||
437 | }; | ||
438 | |||
439 | typedef MotionHelicalTpl<Scalar, Options, axis> JointMotion; | ||
440 | typedef Eigen::Matrix<Scalar, 1, 1, Options> JointForce; | ||
441 | typedef Eigen::Matrix<Scalar, 6, 1, Options> DenseBase; | ||
442 | typedef Eigen::Matrix<Scalar, 1, 1, Options> ReducedSquaredMatrix; | ||
443 | |||
444 | typedef DenseBase MatrixReturnType; | ||
445 | typedef const DenseBase ConstMatrixReturnType; | ||
446 | |||
447 | typedef typename ReducedSquaredMatrix::IdentityReturnType StDiagonalMatrixSOperationReturnType; | ||
448 | }; // traits JointMotionSubspaceHelicalTpl | ||
449 | |||
450 | template<class ConstraintDerived> | ||
451 | struct TransposeConstraintActionConstraint | ||
452 | { | ||
453 | typedef | ||
454 | typename Eigen::Matrix<typename ConstraintDerived::Scalar, 1, 1, ConstraintDerived::Options> | ||
455 | ReturnType; | ||
456 | }; | ||
457 | |||
458 | template<typename _Scalar, int _Options, int axis> | ||
459 | struct JointMotionSubspaceHelicalTpl | ||
460 | : JointMotionSubspaceBase<JointMotionSubspaceHelicalTpl<_Scalar, _Options, axis>> | ||
461 | { | ||
462 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW | ||
463 | |||
464 | PINOCCHIO_CONSTRAINT_TYPEDEF_TPL(JointMotionSubspaceHelicalTpl) | ||
465 | enum | ||
466 | { | ||
467 | NV = 1 | ||
468 | }; | ||
469 | |||
470 | typedef SpatialAxis<ANGULAR + axis> AxisAngular; | ||
471 | typedef SpatialAxis<ANGULAR + axis> AxisLinear; | ||
472 | |||
473 | typedef typename AxisAngular::CartesianAxis3 CartesianAxis3Angular; | ||
474 | typedef typename AxisLinear::CartesianAxis3 CartesianAxis3Linear; | ||
475 | |||
476 | ✗ | JointMotionSubspaceHelicalTpl() | |
477 | { | ||
478 | } | ||
479 | |||
480 | 136 | JointMotionSubspaceHelicalTpl(const Scalar & h) | |
481 | 136 | : m_pitch(h) | |
482 | { | ||
483 | 136 | } | |
484 | |||
485 | template<typename Vector1Like> | ||
486 | 4 | JointMotion __mult__(const Eigen::MatrixBase<Vector1Like> & v) const | |
487 | { | ||
488 | EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector1Like, 1); | ||
489 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 2 times.
|
4 | assert(v.size() == 1); |
490 |
1/2✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
|
4 | return JointMotion(v[0], v[0] * m_pitch); |
491 | } | ||
492 | |||
493 | template<typename S1, int O1> | ||
494 | typename SE3GroupAction<JointMotionSubspaceHelicalTpl>::ReturnType | ||
495 | ✗ | se3Action(const SE3Tpl<S1, O1> & m) const | |
496 | { | ||
497 | typedef typename SE3GroupAction<JointMotionSubspaceHelicalTpl>::ReturnType ReturnType; | ||
498 | ✗ | ReturnType res; | |
499 | ✗ | res.template segment<3>(LINEAR) = | |
500 | ✗ | m.translation().cross(m.rotation().col(axis)) + m_pitch * (m.rotation().col(axis)); | |
501 | ✗ | res.template segment<3>(ANGULAR) = m.rotation().col(axis); | |
502 | ✗ | return res; | |
503 | } | ||
504 | |||
505 | template<typename S1, int O1> | ||
506 | typename SE3GroupAction<JointMotionSubspaceHelicalTpl>::ReturnType | ||
507 | ✗ | se3ActionInverse(const SE3Tpl<S1, O1> & m) const | |
508 | { | ||
509 | typedef typename SE3GroupAction<JointMotionSubspaceHelicalTpl>::ReturnType ReturnType; | ||
510 | typedef typename AxisAngular::CartesianAxis3 CartesianAxis3; | ||
511 | ✗ | ReturnType res; | |
512 | ✗ | res.template segment<3>(LINEAR).noalias() = | |
513 | ✗ | m.rotation().transpose() * CartesianAxis3::cross(m.translation()) | |
514 | ✗ | + m.rotation().transpose().col(axis) * m_pitch; | |
515 | ✗ | res.template segment<3>(ANGULAR) = m.rotation().transpose().col(axis); | |
516 | ✗ | return res; | |
517 | } | ||
518 | |||
519 | ✗ | int nv_impl() const | |
520 | { | ||
521 | ✗ | return NV; | |
522 | } | ||
523 | |||
524 | // For force T | ||
525 | struct TransposeConst : JointMotionSubspaceTransposeBase<JointMotionSubspaceHelicalTpl> | ||
526 | { | ||
527 | const JointMotionSubspaceHelicalTpl & ref; | ||
528 | ✗ | TransposeConst(const JointMotionSubspaceHelicalTpl & ref) | |
529 | ✗ | : ref(ref) | |
530 | { | ||
531 | } | ||
532 | |||
533 | template<typename ForceDerived> | ||
534 | typename ConstraintForceOp<JointMotionSubspaceHelicalTpl, ForceDerived>::ReturnType | ||
535 | ✗ | operator*(const ForceDense<ForceDerived> & f) const | |
536 | { | ||
537 | ✗ | return Eigen::Matrix<Scalar, 1, 1>(f.angular()(axis) + f.linear()(axis) * ref.m_pitch); | |
538 | } | ||
539 | |||
540 | /// [CRBA] MatrixBase operator* (Constraint::Transpose S, ForceSet::Block) | ||
541 | template<typename Derived> | ||
542 | typename ConstraintForceSetOp<JointMotionSubspaceHelicalTpl, Derived>::ReturnType | ||
543 | ✗ | operator*(const Eigen::MatrixBase<Derived> & F) const | |
544 | { | ||
545 | ✗ | assert(F.rows() == 6); | |
546 | ✗ | return F.row(ANGULAR + axis) + F.row(LINEAR + axis) * ref.m_pitch; | |
547 | } | ||
548 | }; // struct TransposeConst | ||
549 | |||
550 | ✗ | TransposeConst transpose() const | |
551 | { | ||
552 | ✗ | return TransposeConst(*this); | |
553 | } | ||
554 | |||
555 | /* CRBA joint operators | ||
556 | * - ForceSet::Block = ForceSet | ||
557 | * - ForceSet operator* (Inertia Y,Constraint S) | ||
558 | * - MatrixBase operator* (Constraint::Transpose S, ForceSet::Block) | ||
559 | * - SE3::act(ForceSet::Block) | ||
560 | */ | ||
561 | ✗ | DenseBase matrix_impl() const | |
562 | { | ||
563 | ✗ | DenseBase S; | |
564 | ✗ | MotionRef<DenseBase> v(S); | |
565 | ✗ | v << AxisAngular(); | |
566 | ✗ | S(LINEAR + axis) = m_pitch; | |
567 | ✗ | return S; | |
568 | } | ||
569 | |||
570 | template<typename MotionDerived> | ||
571 | typename MotionAlgebraAction<JointMotionSubspaceHelicalTpl, MotionDerived>::ReturnType | ||
572 | ✗ | motionAction(const MotionDense<MotionDerived> & m) const | |
573 | { | ||
574 | typedef typename MotionAlgebraAction<JointMotionSubspaceHelicalTpl, MotionDerived>::ReturnType | ||
575 | ReturnType; | ||
576 | ✗ | ReturnType res; | |
577 | // Linear | ||
578 | ✗ | CartesianAxis3Linear::cross(-m.linear(), res.template segment<3>(LINEAR)); | |
579 | ✗ | CartesianAxis3Linear::alphaCross(-m_pitch, m.angular(), res.template segment<3>(ANGULAR)); | |
580 | ✗ | res.template segment<3>(LINEAR) += res.template segment<3>(ANGULAR); | |
581 | |||
582 | // Angular | ||
583 | ✗ | CartesianAxis3Angular::cross(-m.angular(), res.template segment<3>(ANGULAR)); | |
584 | ✗ | return res; | |
585 | } | ||
586 | |||
587 | ✗ | bool isEqual(const JointMotionSubspaceHelicalTpl &) const | |
588 | { | ||
589 | ✗ | return true; | |
590 | } | ||
591 | |||
592 | ✗ | Scalar & h() | |
593 | { | ||
594 | ✗ | return m_pitch; | |
595 | } | ||
596 | ✗ | const Scalar & h() const | |
597 | { | ||
598 | ✗ | return m_pitch; | |
599 | } | ||
600 | |||
601 | protected: | ||
602 | Scalar m_pitch; | ||
603 | }; // struct JointMotionSubspaceHelicalTpl | ||
604 | |||
605 | template<typename _Scalar, int _Options, int _axis> | ||
606 | ✗ | Eigen::Matrix<_Scalar, 1, 1, _Options> operator*( | |
607 | const typename JointMotionSubspaceHelicalTpl<_Scalar, _Options, _axis>::TransposeConst & | ||
608 | S_transpose, | ||
609 | const JointMotionSubspaceHelicalTpl<_Scalar, _Options, _axis> & S) | ||
610 | { | ||
611 | ✗ | Eigen::Matrix<_Scalar, 1, 1, _Options> res; | |
612 | ✗ | res(0) = 1.0 + S_transpose.ref.h() * S.h(); | |
613 | ✗ | return res; | |
614 | } | ||
615 | |||
616 | template<typename _Scalar, int _Options, int _axis> | ||
617 | struct JointHelicalTpl | ||
618 | { | ||
619 | typedef _Scalar Scalar; | ||
620 | |||
621 | enum | ||
622 | { | ||
623 | Options = _Options, | ||
624 | axis = _axis | ||
625 | }; | ||
626 | }; | ||
627 | |||
628 | template<typename S1, int O1, typename S2, int O2, int axis> | ||
629 | struct MultiplicationOp<InertiaTpl<S1, O1>, JointMotionSubspaceHelicalTpl<S2, O2, axis>> | ||
630 | { | ||
631 | typedef Eigen::Matrix<S2, 6, 1, O2> ReturnType; | ||
632 | }; | ||
633 | |||
634 | /* [CRBA] ForceSet operator* (Inertia Y,Constraint S) */ | ||
635 | namespace impl | ||
636 | { | ||
637 | template<typename S1, int O1, typename S2, int O2> | ||
638 | struct LhsMultiplicationOp<InertiaTpl<S1, O1>, JointMotionSubspaceHelicalTpl<S2, O2, 0>> | ||
639 | { | ||
640 | typedef InertiaTpl<S1, O1> Inertia; | ||
641 | typedef JointMotionSubspaceHelicalTpl<S2, O2, 0> Constraint; | ||
642 | typedef typename MultiplicationOp<Inertia, Constraint>::ReturnType ReturnType; | ||
643 | ✗ | static inline ReturnType run(const Inertia & Y, const Constraint & constraint) | |
644 | { | ||
645 | ✗ | ReturnType res; | |
646 | ✗ | const S2 & m_pitch = constraint.h(); | |
647 | |||
648 | /* Y(:,3) = ( 0,-z, y, I00+yy+zz, I01-xy , I02-xz ) */ | ||
649 | /* Y(:,0) = ( 1,0, 0, 0 , z , -y ) */ | ||
650 | ✗ | const S1 &m = Y.mass(), &x = Y.lever()[0], &y = Y.lever()[1], &z = Y.lever()[2]; | |
651 | ✗ | const typename Inertia::Symmetric3 & I = Y.inertia(); | |
652 | |||
653 | ✗ | res << m * m_pitch, -m * z, m * y, I(0, 0) + m * (y * y + z * z), | |
654 | ✗ | I(0, 1) - m * x * y + m * z * m_pitch, I(0, 2) - m * x * z - m * y * m_pitch; | |
655 | |||
656 | ✗ | return res; | |
657 | } | ||
658 | }; | ||
659 | |||
660 | template<typename S1, int O1, typename S2, int O2> | ||
661 | struct LhsMultiplicationOp<InertiaTpl<S1, O1>, JointMotionSubspaceHelicalTpl<S2, O2, 1>> | ||
662 | { | ||
663 | typedef InertiaTpl<S1, O1> Inertia; | ||
664 | typedef JointMotionSubspaceHelicalTpl<S2, O2, 1> Constraint; | ||
665 | typedef typename MultiplicationOp<Inertia, Constraint>::ReturnType ReturnType; | ||
666 | ✗ | static inline ReturnType run(const Inertia & Y, const Constraint & constraint) | |
667 | { | ||
668 | ✗ | ReturnType res; | |
669 | ✗ | const S2 & m_pitch = constraint.h(); | |
670 | |||
671 | /* Y(:,4) = ( z, 0,-x, I10-xy , I11+xx+zz, I12-yz ) */ | ||
672 | /* Y(:,1) = ( 0,1, 0, -z , 0 , x) */ | ||
673 | ✗ | const S1 &m = Y.mass(), &x = Y.lever()[0], &y = Y.lever()[1], &z = Y.lever()[2]; | |
674 | ✗ | const typename Inertia::Symmetric3 & I = Y.inertia(); | |
675 | |||
676 | ✗ | res << m * z, m * m_pitch, -m * x, I(1, 0) - m * x * y - m * z * m_pitch, | |
677 | ✗ | I(1, 1) + m * (x * x + z * z), I(1, 2) - m * y * z + m * x * m_pitch; | |
678 | |||
679 | ✗ | return res; | |
680 | } | ||
681 | }; | ||
682 | |||
683 | template<typename S1, int O1, typename S2, int O2> | ||
684 | struct LhsMultiplicationOp<InertiaTpl<S1, O1>, JointMotionSubspaceHelicalTpl<S2, O2, 2>> | ||
685 | { | ||
686 | typedef InertiaTpl<S1, O1> Inertia; | ||
687 | typedef JointMotionSubspaceHelicalTpl<S2, O2, 2> Constraint; | ||
688 | typedef typename MultiplicationOp<Inertia, Constraint>::ReturnType ReturnType; | ||
689 | ✗ | static inline ReturnType run(const Inertia & Y, const Constraint & constraint) | |
690 | { | ||
691 | ✗ | ReturnType res; | |
692 | ✗ | const S2 & m_pitch = constraint.h(); | |
693 | |||
694 | /* Y(:,5) = (-y, x, 0, I20-xz , I21-yz , I22+xx+yy) */ | ||
695 | /* Y(:,2) = ( 0,0, 1, y , -x , 0) */ | ||
696 | ✗ | const S1 &m = Y.mass(), &x = Y.lever()[0], &y = Y.lever()[1], &z = Y.lever()[2]; | |
697 | ✗ | const typename Inertia::Symmetric3 & I = Y.inertia(); | |
698 | |||
699 | ✗ | res << -m * y, m * x, m * m_pitch, I(2, 0) - m * x * z + m * y * m_pitch, | |
700 | ✗ | I(2, 1) - m * y * z - m * x * m_pitch, I(2, 2) + m * (x * x + y * y); | |
701 | |||
702 | ✗ | return res; | |
703 | } | ||
704 | }; | ||
705 | } // namespace impl | ||
706 | |||
707 | template<typename M6Like, typename S2, int O2, int axis> | ||
708 | struct MultiplicationOp<Eigen::MatrixBase<M6Like>, JointMotionSubspaceHelicalTpl<S2, O2, axis>> | ||
709 | { | ||
710 | typedef Eigen::Matrix<S2, 6, 1> ReturnType; | ||
711 | }; | ||
712 | |||
713 | /* [ABA] operator* (Inertia Y,Constraint S) */ | ||
714 | namespace impl | ||
715 | { | ||
716 | template<typename M6Like, typename Scalar, int Options, int axis> | ||
717 | struct LhsMultiplicationOp< | ||
718 | Eigen::MatrixBase<M6Like>, | ||
719 | JointMotionSubspaceHelicalTpl<Scalar, Options, axis>> | ||
720 | { | ||
721 | typedef JointMotionSubspaceHelicalTpl<Scalar, Options, axis> Constraint; | ||
722 | typedef Eigen::Matrix<Scalar, 6, 1> ReturnType; | ||
723 | static inline ReturnType | ||
724 | ✗ | run(const Eigen::MatrixBase<M6Like> & Y, const Constraint & constraint) | |
725 | { | ||
726 | EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(M6Like, 6, 6); | ||
727 | ✗ | return (Y.col(Inertia::ANGULAR + axis) + Y.col(Inertia::LINEAR + axis) * constraint.h()); | |
728 | } | ||
729 | }; | ||
730 | } // namespace impl | ||
731 | |||
732 | template<typename _Scalar, int _Options, int axis> | ||
733 | struct traits<JointHelicalTpl<_Scalar, _Options, axis>> | ||
734 | { | ||
735 | enum | ||
736 | { | ||
737 | NQ = 1, | ||
738 | NV = 1 | ||
739 | }; | ||
740 | typedef _Scalar Scalar; | ||
741 | enum | ||
742 | { | ||
743 | Options = _Options | ||
744 | }; | ||
745 | typedef JointDataHelicalTpl<Scalar, Options, axis> JointDataDerived; | ||
746 | typedef JointModelHelicalTpl<Scalar, Options, axis> JointModelDerived; | ||
747 | typedef JointMotionSubspaceHelicalTpl<Scalar, Options, axis> Constraint_t; | ||
748 | typedef TransformHelicalTpl<Scalar, Options, axis> Transformation_t; | ||
749 | typedef MotionHelicalTpl<Scalar, Options, axis> Motion_t; | ||
750 | typedef MotionZeroTpl<Scalar, Options> Bias_t; | ||
751 | |||
752 | // [ABA] | ||
753 | typedef Eigen::Matrix<Scalar, 6, NV, Options> U_t; | ||
754 | typedef Eigen::Matrix<Scalar, NV, NV, Options> D_t; | ||
755 | typedef Eigen::Matrix<Scalar, 6, NV, Options> UD_t; | ||
756 | |||
757 | typedef Eigen::Matrix<Scalar, NQ, 1, Options> ConfigVector_t; | ||
758 | typedef Eigen::Matrix<Scalar, NV, 1, Options> TangentVector_t; | ||
759 | |||
760 | PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE | ||
761 | }; | ||
762 | |||
763 | template<typename _Scalar, int _Options, int axis> | ||
764 | struct traits<JointDataHelicalTpl<_Scalar, _Options, axis>> | ||
765 | { | ||
766 | typedef JointHelicalTpl<_Scalar, _Options, axis> JointDerived; | ||
767 | typedef _Scalar Scalar; | ||
768 | }; | ||
769 | |||
770 | template<typename _Scalar, int _Options, int axis> | ||
771 | struct traits<JointModelHelicalTpl<_Scalar, _Options, axis>> | ||
772 | { | ||
773 | typedef JointHelicalTpl<_Scalar, _Options, axis> JointDerived; | ||
774 | typedef _Scalar Scalar; | ||
775 | }; | ||
776 | |||
777 | template<typename _Scalar, int _Options, int axis> | ||
778 | struct JointDataHelicalTpl : public JointDataBase<JointDataHelicalTpl<_Scalar, _Options, axis>> | ||
779 | { | ||
780 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW | ||
781 | typedef JointHelicalTpl<_Scalar, _Options, axis> JointDerived; | ||
782 | PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(JointDerived); | ||
783 | 4 | PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR | |
784 | |||
785 | ConfigVector_t joint_q; | ||
786 | TangentVector_t joint_v; | ||
787 | |||
788 | Constraint_t S; | ||
789 | Transformation_t M; | ||
790 | Motion_t v; | ||
791 | Bias_t c; | ||
792 | |||
793 | // [ABA] specific data | ||
794 | U_t U; | ||
795 | D_t Dinv; | ||
796 | UD_t UDinv; | ||
797 | D_t StU; | ||
798 | |||
799 | 136 | JointDataHelicalTpl() | |
800 |
1/2✓ Branch 2 taken 68 times.
✗ Branch 3 not taken.
|
136 | : joint_q(ConfigVector_t::Zero()) |
801 |
1/2✓ Branch 2 taken 68 times.
✗ Branch 3 not taken.
|
136 | , joint_v(TangentVector_t::Zero()) |
802 | 136 | , S((Scalar)0) | |
803 | 136 | , M((Scalar)0, (Scalar)1, (Scalar)0) | |
804 | 136 | , v((Scalar)0, (Scalar)0) | |
805 |
1/2✓ Branch 2 taken 68 times.
✗ Branch 3 not taken.
|
136 | , U(U_t::Zero()) |
806 |
1/2✓ Branch 2 taken 68 times.
✗ Branch 3 not taken.
|
136 | , Dinv(D_t::Zero()) |
807 |
1/2✓ Branch 2 taken 68 times.
✗ Branch 3 not taken.
|
136 | , UDinv(UD_t::Zero()) |
808 |
1/2✓ Branch 3 taken 68 times.
✗ Branch 4 not taken.
|
272 | , StU(D_t::Zero()) |
809 | { | ||
810 | 136 | } | |
811 | |||
812 | 240 | static std::string classname() | |
813 | { | ||
814 |
2/4✓ Branch 3 taken 120 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 120 times.
✗ Branch 7 not taken.
|
480 | return std::string("JointDataH") + axisLabel<axis>(); |
815 | } | ||
816 | ✗ | std::string shortname() const | |
817 | { | ||
818 | ✗ | return classname(); | |
819 | } | ||
820 | |||
821 | }; // struct JointDataHelicalTpl | ||
822 | |||
823 | template<typename NewScalar, typename Scalar, int Options, int axis> | ||
824 | struct CastType<NewScalar, JointModelHelicalTpl<Scalar, Options, axis>> | ||
825 | { | ||
826 | typedef JointModelHelicalTpl<NewScalar, Options, axis> type; | ||
827 | }; | ||
828 | |||
829 | template<typename _Scalar, int _Options, int axis> | ||
830 | struct JointModelHelicalTpl : public JointModelBase<JointModelHelicalTpl<_Scalar, _Options, axis>> | ||
831 | { | ||
832 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW | ||
833 | typedef JointHelicalTpl<_Scalar, _Options, axis> JointDerived; | ||
834 | PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived); | ||
835 | |||
836 | typedef JointModelBase<JointModelHelicalTpl> Base; | ||
837 | using Base::id; | ||
838 | using Base::idx_q; | ||
839 | using Base::idx_v; | ||
840 | using Base::setIndexes; | ||
841 | |||
842 | typedef Eigen::Matrix<Scalar, 3, 1, _Options> Vector3; | ||
843 | |||
844 | 16 | JointDataDerived createData() const | |
845 | { | ||
846 | 16 | return JointDataDerived(); | |
847 | } | ||
848 | |||
849 | 120 | JointModelHelicalTpl() | |
850 | 120 | { | |
851 | 120 | } | |
852 | |||
853 | ✗ | explicit JointModelHelicalTpl(const Scalar & h) | |
854 | ✗ | : m_pitch(h) | |
855 | { | ||
856 | } | ||
857 | |||
858 | ✗ | const std::vector<bool> hasConfigurationLimit() const | |
859 | { | ||
860 | ✗ | return {true, true}; | |
861 | } | ||
862 | |||
863 | ✗ | const std::vector<bool> hasConfigurationLimitInTangent() const | |
864 | { | ||
865 | ✗ | return {true, true}; | |
866 | } | ||
867 | |||
868 | template<typename ConfigVector> | ||
869 | EIGEN_DONT_INLINE void | ||
870 | ✗ | calc(JointDataDerived & data, const typename Eigen::MatrixBase<ConfigVector> & qs) const | |
871 | { | ||
872 | ✗ | data.joint_q[0] = qs[idx_q()]; | |
873 | Scalar ca, sa; | ||
874 | ✗ | SINCOS(data.joint_q[0], &sa, &ca); | |
875 | ✗ | data.M.setValues(sa, ca, data.joint_q[0] * m_pitch); | |
876 | ✗ | data.S.h() = m_pitch; | |
877 | } | ||
878 | |||
879 | template<typename TangentVector> | ||
880 | EIGEN_DONT_INLINE void | ||
881 | ✗ | calc(JointDataDerived & data, const Blank, const typename Eigen::MatrixBase<TangentVector> & vs) | |
882 | const | ||
883 | { | ||
884 | ✗ | data.joint_v[0] = vs[idx_v()]; | |
885 | ✗ | data.v.angularRate() = data.joint_v[0]; | |
886 | ✗ | data.v.linearRate() = data.joint_v[0] * m_pitch; | |
887 | } | ||
888 | |||
889 | template<typename ConfigVector, typename TangentVector> | ||
890 | 12 | EIGEN_DONT_INLINE void calc( | |
891 | JointDataDerived & data, | ||
892 | const typename Eigen::MatrixBase<ConfigVector> & qs, | ||
893 | const typename Eigen::MatrixBase<TangentVector> & vs) const | ||
894 | { | ||
895 | 12 | calc(data, qs.derived()); | |
896 | |||
897 | 12 | data.joint_v[0] = vs[idx_v()]; | |
898 | 12 | data.v.angularRate() = data.joint_v[0]; | |
899 | 12 | data.v.linearRate() = data.joint_v[0] * m_pitch; | |
900 | } | ||
901 | |||
902 | template<typename VectorLike, typename Matrix6Like> | ||
903 | ✗ | void calc_aba( | |
904 | JointDataDerived & data, | ||
905 | const Eigen::MatrixBase<VectorLike> & armature, | ||
906 | const Eigen::MatrixBase<Matrix6Like> & I, | ||
907 | const bool update_I) const | ||
908 | { | ||
909 | ✗ | data.U = I.col(Inertia::ANGULAR + axis) + m_pitch * I.col(Inertia::LINEAR + axis); | |
910 | ✗ | data.StU[0] = | |
911 | ✗ | data.U(Inertia::ANGULAR + axis) + m_pitch * data.U(Inertia::LINEAR + axis) + armature[0]; | |
912 | ✗ | data.Dinv[0] = Scalar(1) / data.StU[0]; | |
913 | ✗ | data.UDinv.noalias() = data.U * data.Dinv; | |
914 | |||
915 | ✗ | if (update_I) | |
916 | ✗ | PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like, I).noalias() -= data.UDinv * data.U.transpose(); | |
917 | } | ||
918 | |||
919 | 240 | static std::string classname() | |
920 | { | ||
921 |
2/4✓ Branch 3 taken 120 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 120 times.
✗ Branch 7 not taken.
|
480 | return std::string("JointModelH") + axisLabel<axis>(); |
922 | } | ||
923 | ✗ | std::string shortname() const | |
924 | { | ||
925 | ✗ | return classname(); | |
926 | } | ||
927 | |||
928 | ✗ | Vector3 getMotionAxis() const | |
929 | { | ||
930 | switch (axis) | ||
931 | { | ||
932 | case 0: | ||
933 | ✗ | return Vector3::UnitX(); | |
934 | case 1: | ||
935 | ✗ | return Vector3::UnitY(); | |
936 | case 2: | ||
937 | ✗ | return Vector3::UnitZ(); | |
938 | default: | ||
939 | assert(false && "must never happen"); | ||
940 | break; | ||
941 | } | ||
942 | } | ||
943 | |||
944 | /// \returns An expression of *this with the Scalar type casted to NewScalar. | ||
945 | template<typename NewScalar> | ||
946 | ✗ | JointModelHelicalTpl<NewScalar, Options, axis> cast() const | |
947 | { | ||
948 | typedef JointModelHelicalTpl<NewScalar, Options, axis> ReturnType; | ||
949 | ✗ | ReturnType res(ScalarCast<NewScalar, Scalar>::cast(m_pitch)); | |
950 | ✗ | res.setIndexes(id(), idx_q(), idx_v()); | |
951 | ✗ | return res; | |
952 | } | ||
953 | |||
954 | Scalar m_pitch; | ||
955 | |||
956 | }; // struct JointModelHelicalTpl | ||
957 | |||
958 | typedef JointHelicalTpl<context::Scalar, context::Options, 0> JointHX; | ||
959 | typedef JointDataHelicalTpl<context::Scalar, context::Options, 0> JointDataHX; | ||
960 | typedef JointModelHelicalTpl<context::Scalar, context::Options, 0> JointModelHX; | ||
961 | |||
962 | typedef JointHelicalTpl<context::Scalar, context::Options, 1> JointHY; | ||
963 | typedef JointDataHelicalTpl<context::Scalar, context::Options, 1> JointDataHY; | ||
964 | typedef JointModelHelicalTpl<context::Scalar, context::Options, 1> JointModelHY; | ||
965 | |||
966 | typedef JointHelicalTpl<context::Scalar, context::Options, 2> JointHZ; | ||
967 | typedef JointDataHelicalTpl<context::Scalar, context::Options, 2> JointDataHZ; | ||
968 | typedef JointModelHelicalTpl<context::Scalar, context::Options, 2> JointModelHZ; | ||
969 | |||
970 | } // namespace pinocchio | ||
971 | |||
972 | #include <boost/type_traits.hpp> | ||
973 | |||
974 | namespace boost | ||
975 | { | ||
976 | template<typename Scalar, int Options, int axis> | ||
977 | struct has_nothrow_constructor<::pinocchio::JointModelHelicalTpl<Scalar, Options, axis>> | ||
978 | : public integral_constant<bool, true> | ||
979 | { | ||
980 | }; | ||
981 | |||
982 | template<typename Scalar, int Options, int axis> | ||
983 | struct has_nothrow_copy<::pinocchio::JointModelHelicalTpl<Scalar, Options, axis>> | ||
984 | : public integral_constant<bool, true> | ||
985 | { | ||
986 | }; | ||
987 | |||
988 | template<typename Scalar, int Options, int axis> | ||
989 | struct has_nothrow_constructor<::pinocchio::JointDataHelicalTpl<Scalar, Options, axis>> | ||
990 | : public integral_constant<bool, true> | ||
991 | { | ||
992 | }; | ||
993 | |||
994 | template<typename Scalar, int Options, int axis> | ||
995 | struct has_nothrow_copy<::pinocchio::JointDataHelicalTpl<Scalar, Options, axis>> | ||
996 | : public integral_constant<bool, true> | ||
997 | { | ||
998 | }; | ||
999 | } // namespace boost | ||
1000 | |||
1001 | #endif // ifndef __pinocchio_multibody_joint_helical_hpp__ | ||
1002 |