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