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