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