Directory: | ./ |
---|---|
File: | include/pinocchio/multibody/joint/joint-universal.hpp |
Date: | 2024-08-27 18:20:05 |
Exec | Total | Coverage | |
---|---|---|---|
Lines: | 59 | 168 | 35.1% |
Branches: | 99 | 466 | 21.2% |
Line | Branch | Exec | Source |
---|---|---|---|
1 | // | ||
2 | // Copyright (c) 2023 INRIA | ||
3 | // | ||
4 | |||
5 | #ifndef __pinocchio_multibody_joint_universal_hpp__ | ||
6 | #define __pinocchio_multibody_joint_universal_hpp__ | ||
7 | |||
8 | #include "pinocchio/fwd.hpp" | ||
9 | #include "pinocchio/multibody/joint/joint-base.hpp" | ||
10 | #include "pinocchio/multibody/joint/joint-spherical.hpp" | ||
11 | #include "pinocchio/multibody/joint-motion-subspace.hpp" | ||
12 | #include "pinocchio/spatial/inertia.hpp" | ||
13 | #include "pinocchio/utils/check.hpp" | ||
14 | |||
15 | #include "pinocchio/math/matrix.hpp" | ||
16 | #include "pinocchio/math/rotation.hpp" | ||
17 | |||
18 | namespace pinocchio | ||
19 | { | ||
20 | template<typename Scalar, int Options> | ||
21 | struct JointMotionSubspaceUniversalTpl; | ||
22 | |||
23 | template<typename _Scalar, int _Options> | ||
24 | struct traits<JointMotionSubspaceUniversalTpl<_Scalar, _Options>> | ||
25 | { | ||
26 | typedef _Scalar Scalar; | ||
27 | enum | ||
28 | { | ||
29 | Options = _Options | ||
30 | }; | ||
31 | enum | ||
32 | { | ||
33 | LINEAR = 0, | ||
34 | ANGULAR = 3 | ||
35 | }; | ||
36 | |||
37 | typedef MotionSphericalTpl<Scalar, Options> JointMotion; | ||
38 | typedef Eigen::Matrix<Scalar, 2, 1, Options> JointForce; | ||
39 | typedef Eigen::Matrix<Scalar, 6, 2, Options> DenseBase; | ||
40 | typedef Eigen::Matrix<Scalar, 3, 3, Options> ReducedSquaredMatrix; | ||
41 | |||
42 | typedef DenseBase MatrixReturnType; | ||
43 | typedef const DenseBase ConstMatrixReturnType; | ||
44 | |||
45 | typedef Eigen::Matrix<Scalar, 3, 1, Options> Vector3; | ||
46 | typedef Eigen::Matrix<Scalar, 3, 2, Options> Matrix32; | ||
47 | typedef Eigen::Matrix<Scalar, 2, 2, Options> Matrix2; | ||
48 | |||
49 | typedef Matrix2 StDiagonalMatrixSOperationReturnType; | ||
50 | }; // traits JointMotionSubspaceUniversalTpl | ||
51 | |||
52 | template<typename Scalar, int Options> | ||
53 | struct SE3GroupAction<JointMotionSubspaceUniversalTpl<Scalar, Options>> | ||
54 | { | ||
55 | typedef Eigen::Matrix<Scalar, 6, 2, Options> ReturnType; | ||
56 | }; | ||
57 | |||
58 | template<typename Scalar, int Options, typename MotionDerived> | ||
59 | struct MotionAlgebraAction<JointMotionSubspaceUniversalTpl<Scalar, Options>, MotionDerived> | ||
60 | { | ||
61 | typedef Eigen::Matrix<Scalar, 6, 2, Options> ReturnType; | ||
62 | }; | ||
63 | |||
64 | template<typename Scalar, int Options, typename ForceDerived> | ||
65 | struct ConstraintForceOp<JointMotionSubspaceUniversalTpl<Scalar, Options>, ForceDerived> | ||
66 | { | ||
67 | typedef typename traits<JointMotionSubspaceUniversalTpl<Scalar, Options>>::Vector3 Vector3; | ||
68 | typedef Eigen::Matrix< | ||
69 | typename PINOCCHIO_EIGEN_DOT_PRODUCT_RETURN_TYPE( | ||
70 | Vector3, typename ForceDense<ForceDerived>::ConstAngularType), | ||
71 | 2, | ||
72 | 1, | ||
73 | Options> | ||
74 | ReturnType; | ||
75 | }; | ||
76 | |||
77 | template<typename Scalar, int Options, typename ForceSet> | ||
78 | struct ConstraintForceSetOp<JointMotionSubspaceUniversalTpl<Scalar, Options>, ForceSet> | ||
79 | { | ||
80 | typedef typename traits<JointMotionSubspaceUniversalTpl<Scalar, Options>>::Matrix32 Matrix32; | ||
81 | typedef typename MatrixMatrixProduct< | ||
82 | Eigen::Transpose<const Matrix32>, | ||
83 | typename Eigen::MatrixBase<const ForceSet>::template NRowsBlockXpr<3>::Type>::type ReturnType; | ||
84 | }; | ||
85 | |||
86 | template<typename _Scalar, int _Options> | ||
87 | struct JointMotionSubspaceUniversalTpl | ||
88 | : JointMotionSubspaceBase<JointMotionSubspaceUniversalTpl<_Scalar, _Options>> | ||
89 | { | ||
90 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW | ||
91 | PINOCCHIO_CONSTRAINT_TYPEDEF_TPL(JointMotionSubspaceUniversalTpl) | ||
92 | |||
93 | enum | ||
94 | { | ||
95 | NV = 2 | ||
96 | }; | ||
97 | |||
98 | typedef typename traits<JointMotionSubspaceUniversalTpl>::Vector3 Vector3; | ||
99 | typedef typename traits<JointMotionSubspaceUniversalTpl>::Matrix32 Matrix32; | ||
100 | |||
101 | ✗ | JointMotionSubspaceUniversalTpl() | |
102 | { | ||
103 | } | ||
104 | |||
105 | template<typename Matrix32Like> | ||
106 | 24 | JointMotionSubspaceUniversalTpl(const Eigen::MatrixBase<Matrix32Like> & subspace) | |
107 | 24 | : m_S(subspace) | |
108 | { | ||
109 | 24 | } | |
110 | |||
111 | template<typename Vector3Like> | ||
112 | 2 | JointMotion __mult__(const Eigen::MatrixBase<Vector3Like> & v) const | |
113 | { | ||
114 | // EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3Like,3); | ||
115 |
1/2✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
|
4 | return JointMotion(m_S * v); |
116 | } | ||
117 | |||
118 | template<typename S1, int O1> | ||
119 | typename SE3GroupAction<JointMotionSubspaceUniversalTpl>::ReturnType | ||
120 | ✗ | se3Action(const SE3Tpl<S1, O1> & m) const | |
121 | { | ||
122 | typedef typename SE3GroupAction<JointMotionSubspaceUniversalTpl>::ReturnType ReturnType; | ||
123 | |||
124 | ✗ | ReturnType res; | |
125 | ✗ | res.template middleRows<3>(ANGULAR).noalias() = m.rotation() * m_S; | |
126 | ✗ | cross( | |
127 | ✗ | m.translation(), res.template middleRows<3>(Motion::ANGULAR), | |
128 | ✗ | res.template middleRows<3>(LINEAR)); | |
129 | ✗ | return res; | |
130 | } | ||
131 | |||
132 | template<typename S1, int O1> | ||
133 | typename SE3GroupAction<JointMotionSubspaceUniversalTpl>::ReturnType | ||
134 | ✗ | se3ActionInverse(const SE3Tpl<S1, O1> & m) const | |
135 | { | ||
136 | typedef typename SE3GroupAction<JointMotionSubspaceUniversalTpl>::ReturnType ReturnType; | ||
137 | |||
138 | ✗ | ReturnType res; | |
139 | ✗ | cross(m.translation(), m_S, res.template middleRows<3>(ANGULAR)); | |
140 | ✗ | res.template middleRows<3>(LINEAR).noalias() = | |
141 | ✗ | -m.rotation().transpose() * res.template middleRows<3>(ANGULAR); | |
142 | |||
143 | // ANGULAR | ||
144 | ✗ | res.template middleRows<3>(ANGULAR).noalias() = m.rotation().transpose() * m_S; | |
145 | ✗ | return res; | |
146 | } | ||
147 | |||
148 | ✗ | int nv_impl() const | |
149 | { | ||
150 | ✗ | return NV; | |
151 | } | ||
152 | |||
153 | struct TransposeConst : JointMotionSubspaceTransposeBase<JointMotionSubspaceUniversalTpl> | ||
154 | { | ||
155 | const JointMotionSubspaceUniversalTpl & ref; | ||
156 | ✗ | TransposeConst(const JointMotionSubspaceUniversalTpl & ref) | |
157 | ✗ | : ref(ref) | |
158 | { | ||
159 | } | ||
160 | |||
161 | template<typename ForceDerived> | ||
162 | typename ConstraintForceOp<JointMotionSubspaceUniversalTpl, ForceDerived>::ReturnType | ||
163 | ✗ | operator*(const ForceDense<ForceDerived> & f) const | |
164 | { | ||
165 | ✗ | return ref.m_S.transpose() * f.angular(); | |
166 | } | ||
167 | |||
168 | /* [CRBA] MatrixBase operator* (Constraint::Transpose S, ForceSet::Block) */ | ||
169 | template<typename ForceSet> | ||
170 | typename ConstraintForceSetOp<JointMotionSubspaceUniversalTpl, ForceSet>::ReturnType | ||
171 | ✗ | operator*(const Eigen::MatrixBase<ForceSet> & F) | |
172 | { | ||
173 | EIGEN_STATIC_ASSERT( | ||
174 | ForceSet::RowsAtCompileTime == 6, THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE) | ||
175 | /* Return ax.T * F[3:end,:] */ | ||
176 | ✗ | return ref.m_S.transpose() * F.template middleRows<3>(ANGULAR); | |
177 | } | ||
178 | }; | ||
179 | |||
180 | ✗ | TransposeConst transpose() const | |
181 | { | ||
182 | ✗ | return TransposeConst(*this); | |
183 | } | ||
184 | |||
185 | /* CRBA joint operators | ||
186 | * - ForceSet::Block = ForceSet | ||
187 | * - ForceSet operator* (Inertia Y,Constraint S) | ||
188 | * - MatrixBase operator* (Constraint::Transpose S, ForceSet::Block) | ||
189 | * - SE3::act(ForceSet::Block) | ||
190 | */ | ||
191 | ✗ | DenseBase matrix_impl() const | |
192 | { | ||
193 | ✗ | DenseBase S; | |
194 | ✗ | S.template middleRows<3>(LINEAR).setZero(); | |
195 | ✗ | S.template middleRows<3>(ANGULAR) = m_S; | |
196 | ✗ | return S; | |
197 | } | ||
198 | |||
199 | template<typename MotionDerived> | ||
200 | typename MotionAlgebraAction<JointMotionSubspaceUniversalTpl, MotionDerived>::ReturnType | ||
201 | ✗ | motionAction(const MotionDense<MotionDerived> & m) const | |
202 | { | ||
203 | ✗ | const typename MotionDerived::ConstLinearType v = m.linear(); | |
204 | ✗ | const typename MotionDerived::ConstAngularType w = m.angular(); | |
205 | |||
206 | ✗ | DenseBase res; | |
207 | ✗ | cross(v, m_S, res.template middleRows<3>(LINEAR)); | |
208 | ✗ | cross(w, m_S, res.template middleRows<3>(ANGULAR)); | |
209 | ✗ | return res; | |
210 | } | ||
211 | |||
212 | ✗ | const Matrix32 & angularSubspace() const | |
213 | { | ||
214 | ✗ | return m_S; | |
215 | } | ||
216 | ✗ | Matrix32 & angularSubspace() | |
217 | { | ||
218 | ✗ | return m_S; | |
219 | } | ||
220 | |||
221 | ✗ | bool isEqual(const JointMotionSubspaceUniversalTpl & other) const | |
222 | { | ||
223 | ✗ | return internal::comparison_eq(m_S, other.m_S); | |
224 | } | ||
225 | |||
226 | protected: | ||
227 | Matrix32 m_S; | ||
228 | |||
229 | }; // struct JointMotionSubspaceUniversalTpl | ||
230 | |||
231 | namespace details | ||
232 | { | ||
233 | template<typename Scalar, int Options> | ||
234 | struct StDiagonalMatrixSOperation<JointMotionSubspaceUniversalTpl<Scalar, Options>> | ||
235 | { | ||
236 | typedef JointMotionSubspaceUniversalTpl<Scalar, Options> Constraint; | ||
237 | typedef typename traits<Constraint>::StDiagonalMatrixSOperationReturnType ReturnType; | ||
238 | |||
239 | ✗ | static ReturnType run(const JointMotionSubspaceBase<Constraint> & constraint) | |
240 | { | ||
241 | ✗ | return constraint.matrix().transpose() * constraint.matrix(); | |
242 | } | ||
243 | }; | ||
244 | } // namespace details | ||
245 | |||
246 | template<typename S1, int O1, typename S2, int O2> | ||
247 | struct MultiplicationOp<InertiaTpl<S1, O1>, JointMotionSubspaceUniversalTpl<S2, O2>> | ||
248 | { | ||
249 | typedef Eigen::Matrix<S2, 6, 2, O2> ReturnType; | ||
250 | }; | ||
251 | |||
252 | /* [CRBA] ForceSet operator* (Inertia Y,Constraint S) */ | ||
253 | namespace impl | ||
254 | { | ||
255 | template<typename S1, int O1, typename S2, int O2> | ||
256 | struct LhsMultiplicationOp<InertiaTpl<S1, O1>, JointMotionSubspaceUniversalTpl<S2, O2>> | ||
257 | { | ||
258 | typedef InertiaTpl<S1, O1> Inertia; | ||
259 | typedef JointMotionSubspaceUniversalTpl<S2, O2> Constraint; | ||
260 | typedef typename MultiplicationOp<Inertia, Constraint>::ReturnType ReturnType; | ||
261 | ✗ | static inline ReturnType run(const Inertia & Y, const Constraint & cru) | |
262 | { | ||
263 | typedef typename InertiaTpl<S1, O1>::Symmetric3 Symmetric3; | ||
264 | ✗ | Eigen::Matrix<S1, 6, 3, O1> M; | |
265 | ✗ | alphaSkew(-Y.mass(), Y.lever(), M.template middleRows<3>(Constraint::LINEAR)); | |
266 | ✗ | M.template middleRows<3>(Constraint::ANGULAR) = | |
267 | ✗ | (Y.inertia() - typename Symmetric3::AlphaSkewSquare(Y.mass(), Y.lever())).matrix(); | |
268 | |||
269 | ✗ | return (M * cru.angularSubspace()).eval(); | |
270 | } | ||
271 | }; | ||
272 | } // namespace impl | ||
273 | |||
274 | template<typename M6Like, typename Scalar, int Options> | ||
275 | struct MultiplicationOp< | ||
276 | Eigen::MatrixBase<M6Like>, | ||
277 | JointMotionSubspaceUniversalTpl<Scalar, Options>> | ||
278 | { | ||
279 | typedef typename SizeDepType<3>::ColsReturn<M6Like>::ConstType M6LikeCols; | ||
280 | typedef typename Eigen::internal::remove_const<M6LikeCols>::type M6LikeColsNonConst; | ||
281 | |||
282 | typedef JointMotionSubspaceUniversalTpl<Scalar, Options> Constraint; | ||
283 | typedef typename Constraint::Matrix32 Matrix32; | ||
284 | typedef const typename MatrixMatrixProduct<M6LikeColsNonConst, Matrix32>::type ReturnType; | ||
285 | }; | ||
286 | |||
287 | /* [ABA] operator* (Inertia Y,Constraint S) */ | ||
288 | namespace impl | ||
289 | { | ||
290 | template<typename M6Like, typename Scalar, int Options> | ||
291 | struct LhsMultiplicationOp< | ||
292 | Eigen::MatrixBase<M6Like>, | ||
293 | JointMotionSubspaceUniversalTpl<Scalar, Options>> | ||
294 | { | ||
295 | typedef JointMotionSubspaceUniversalTpl<Scalar, Options> Constraint; | ||
296 | typedef | ||
297 | typename MultiplicationOp<Eigen::MatrixBase<M6Like>, Constraint>::ReturnType ReturnType; | ||
298 | |||
299 | ✗ | static inline ReturnType run(const Eigen::MatrixBase<M6Like> & Y, const Constraint & cru) | |
300 | { | ||
301 | EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(M6Like, 6, 6); | ||
302 | ✗ | return Y.derived().template middleCols<3>(Constraint::ANGULAR) * cru.angularSubspace(); | |
303 | } | ||
304 | }; | ||
305 | } // namespace impl | ||
306 | |||
307 | template<typename Scalar, int Options> | ||
308 | struct JointUniversalTpl; | ||
309 | |||
310 | template<typename _Scalar, int _Options> | ||
311 | struct traits<JointUniversalTpl<_Scalar, _Options>> | ||
312 | { | ||
313 | enum | ||
314 | { | ||
315 | NQ = 2, | ||
316 | NV = 2 | ||
317 | }; | ||
318 | typedef _Scalar Scalar; | ||
319 | enum | ||
320 | { | ||
321 | Options = _Options | ||
322 | }; | ||
323 | typedef JointDataUniversalTpl<Scalar, Options> JointDataDerived; | ||
324 | typedef JointModelUniversalTpl<Scalar, Options> JointModelDerived; | ||
325 | typedef JointMotionSubspaceUniversalTpl<Scalar, Options> Constraint_t; | ||
326 | typedef SE3Tpl<Scalar, Options> Transformation_t; | ||
327 | typedef MotionSphericalTpl<Scalar, Options> Motion_t; | ||
328 | typedef MotionSphericalTpl<Scalar, Options> Bias_t; | ||
329 | |||
330 | // [ABA] | ||
331 | typedef Eigen::Matrix<Scalar, 6, NV, Options> U_t; | ||
332 | typedef Eigen::Matrix<Scalar, NV, NV, Options> D_t; | ||
333 | typedef Eigen::Matrix<Scalar, 6, NV, Options> UD_t; | ||
334 | |||
335 | typedef Eigen::Matrix<Scalar, NQ, 1, Options> ConfigVector_t; | ||
336 | typedef Eigen::Matrix<Scalar, NV, 1, Options> TangentVector_t; | ||
337 | |||
338 | PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE | ||
339 | }; | ||
340 | |||
341 | template<typename _Scalar, int _Options> | ||
342 | struct traits<JointDataUniversalTpl<_Scalar, _Options>> | ||
343 | { | ||
344 | typedef JointUniversalTpl<_Scalar, _Options> JointDerived; | ||
345 | typedef _Scalar Scalar; | ||
346 | }; | ||
347 | |||
348 | template<typename _Scalar, int _Options> | ||
349 | struct traits<JointModelUniversalTpl<_Scalar, _Options>> | ||
350 | { | ||
351 | typedef JointUniversalTpl<_Scalar, _Options> JointDerived; | ||
352 | typedef _Scalar Scalar; | ||
353 | }; | ||
354 | |||
355 | template<typename _Scalar, int _Options> | ||
356 | struct JointDataUniversalTpl : public JointDataBase<JointDataUniversalTpl<_Scalar, _Options>> | ||
357 | { | ||
358 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW | ||
359 | typedef JointUniversalTpl<_Scalar, _Options> JointDerived; | ||
360 | PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(JointDerived); | ||
361 | 2 | PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR | |
362 | |||
363 | ConfigVector_t joint_q; | ||
364 | TangentVector_t joint_v; | ||
365 | |||
366 | Transformation_t M; | ||
367 | Constraint_t S; | ||
368 | Motion_t v; | ||
369 | Bias_t c; | ||
370 | |||
371 | // [ABA] specific data | ||
372 | U_t U; | ||
373 | D_t Dinv; | ||
374 | UD_t UDinv; | ||
375 | D_t StU; | ||
376 | |||
377 | 24 | JointDataUniversalTpl() | |
378 |
1/2✓ Branch 2 taken 24 times.
✗ Branch 3 not taken.
|
24 | : joint_q(ConfigVector_t::Zero()) |
379 |
1/2✓ Branch 2 taken 24 times.
✗ Branch 3 not taken.
|
24 | , joint_v(TangentVector_t::Zero()) |
380 | 24 | , M(Transformation_t::Identity()) | |
381 |
1/2✓ Branch 2 taken 24 times.
✗ Branch 3 not taken.
|
24 | , S(Constraint_t::Matrix32::Zero()) |
382 |
1/2✓ Branch 2 taken 24 times.
✗ Branch 3 not taken.
|
24 | , v(Motion_t::Vector3::Zero()) |
383 |
1/2✓ Branch 2 taken 24 times.
✗ Branch 3 not taken.
|
24 | , c(Bias_t::Vector3::Zero()) |
384 |
1/2✓ Branch 2 taken 24 times.
✗ Branch 3 not taken.
|
24 | , U(U_t::Zero()) |
385 |
1/2✓ Branch 2 taken 24 times.
✗ Branch 3 not taken.
|
24 | , Dinv(D_t::Zero()) |
386 |
1/2✓ Branch 2 taken 24 times.
✗ Branch 3 not taken.
|
24 | , UDinv(UD_t::Zero()) |
387 |
1/2✓ Branch 3 taken 24 times.
✗ Branch 4 not taken.
|
48 | , StU(D_t::Zero()) |
388 | { | ||
389 | 24 | } | |
390 | |||
391 | 40 | static std::string classname() | |
392 | { | ||
393 |
1/2✓ Branch 2 taken 40 times.
✗ Branch 3 not taken.
|
40 | return std::string("JointDataUniversal"); |
394 | } | ||
395 | ✗ | std::string shortname() const | |
396 | { | ||
397 | ✗ | return classname(); | |
398 | } | ||
399 | |||
400 | }; // struct JointDataUniversalTpl | ||
401 | |||
402 | PINOCCHIO_JOINT_CAST_TYPE_SPECIALIZATION(JointModelUniversalTpl); | ||
403 | template<typename _Scalar, int _Options> | ||
404 | struct JointModelUniversalTpl : public JointModelBase<JointModelUniversalTpl<_Scalar, _Options>> | ||
405 | { | ||
406 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW | ||
407 | typedef JointUniversalTpl<_Scalar, _Options> JointDerived; | ||
408 | PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived); | ||
409 | typedef Eigen::Matrix<Scalar, 3, 1, _Options> Vector3; | ||
410 | typedef Eigen::Matrix<Scalar, 3, 3, _Options> Matrix3; | ||
411 | |||
412 | typedef JointModelBase<JointModelUniversalTpl> Base; | ||
413 | using Base::id; | ||
414 | using Base::idx_q; | ||
415 | using Base::idx_v; | ||
416 | using Base::setIndexes; | ||
417 | |||
418 | 20 | JointModelUniversalTpl() | |
419 | 20 | { | |
420 | 20 | } | |
421 | |||
422 | ✗ | JointModelUniversalTpl( | |
423 | const Scalar & x1, | ||
424 | const Scalar & y1, | ||
425 | const Scalar & z1, | ||
426 | const Scalar & x2, | ||
427 | const Scalar & y2, | ||
428 | const Scalar & z2) | ||
429 | ✗ | : axis1(x1, y1, z1) | |
430 | ✗ | , axis2(x2, y2, z2) | |
431 | { | ||
432 | ✗ | assert(isUnitary(axis1) && "First Rotation axis is not unitary"); | |
433 | ✗ | assert(isUnitary(axis2) && "Second Rotation axis is not unitary"); | |
434 | ✗ | assert(check_expression_if_real<Scalar>(axis1.dot(axis2) == 0) && "Axii are not orthogonal"); | |
435 | } | ||
436 | |||
437 | template<typename Vector3Like> | ||
438 | ✗ | JointModelUniversalTpl( | |
439 | const Eigen::MatrixBase<Vector3Like> & axis1_, const Eigen::MatrixBase<Vector3Like> & axis2_) | ||
440 | ✗ | : axis1(axis1_) | |
441 | ✗ | , axis2(axis2_) | |
442 | { | ||
443 | EIGEN_STATIC_ASSERT_VECTOR_ONLY(Vector3Like); | ||
444 | ✗ | assert(isUnitary(axis1) && "First Rotation axis is not unitary"); | |
445 | ✗ | assert(isUnitary(axis2) && "Second Rotation axis is not unitary"); | |
446 | ✗ | assert( | |
447 | check_expression_if_real<Scalar>(axis1.dot(axis2) == Scalar(0)) | ||
448 | && "Axii are not orthogonal"); | ||
449 | } | ||
450 | |||
451 | 4 | JointDataDerived createData() const | |
452 | { | ||
453 | 4 | return JointDataDerived(); | |
454 | } | ||
455 | |||
456 | ✗ | const std::vector<bool> hasConfigurationLimit() const | |
457 | { | ||
458 | ✗ | return {true, true}; | |
459 | } | ||
460 | |||
461 | ✗ | const std::vector<bool> hasConfigurationLimitInTangent() const | |
462 | { | ||
463 | ✗ | return {true, true}; | |
464 | } | ||
465 | |||
466 | using Base::isEqual; | ||
467 | ✗ | bool isEqual(const JointModelUniversalTpl & other) const | |
468 | { | ||
469 | ✗ | return Base::isEqual(other) && internal::comparison_eq(axis1, other.axis1) | |
470 | ✗ | && internal::comparison_eq(axis2, other.axis2); | |
471 | } | ||
472 | |||
473 | template<typename ConfigVector> | ||
474 | ✗ | void calc(JointDataDerived & data, const typename Eigen::MatrixBase<ConfigVector> & qs) const | |
475 | { | ||
476 | ✗ | data.joint_q = qs.template segment<NQ>(idx_q()); | |
477 | Scalar c0, s0; | ||
478 | ✗ | SINCOS(data.joint_q(0), &s0, &c0); | |
479 | Scalar c1, s1; | ||
480 | ✗ | SINCOS(data.joint_q(1), &s1, &c1); | |
481 | |||
482 | ✗ | Matrix3 rot1, rot2; | |
483 | ✗ | toRotationMatrix(axis1, c0, s0, rot1); | |
484 | ✗ | toRotationMatrix(axis2, c1, s1, rot2); | |
485 | ✗ | data.M.rotation() = rot1 * rot2; | |
486 | |||
487 | ✗ | data.S.angularSubspace() << rot2.coeffRef(0, 0) * axis1.x() + rot2.coeffRef(1, 0) * axis1.y() | |
488 | ✗ | + rot2.coeffRef(2, 0) * axis1.z(), | |
489 | ✗ | axis2.x(), | |
490 | ✗ | rot2.coeffRef(0, 1) * axis1.x() + rot2.coeffRef(1, 1) * axis1.y() | |
491 | ✗ | + rot2.coeffRef(2, 1) * axis1.z(), | |
492 | ✗ | axis2.y(), | |
493 | ✗ | rot2.coeffRef(0, 2) * axis1.x() + rot2.coeffRef(1, 2) * axis1.y() | |
494 | ✗ | + rot2.coeffRef(2, 2) * axis1.z(), | |
495 | ✗ | axis2.z(); | |
496 | } | ||
497 | |||
498 | template<typename TangentVector> | ||
499 | void | ||
500 | ✗ | calc(JointDataDerived & data, const Blank, const typename Eigen::MatrixBase<TangentVector> & vs) | |
501 | const | ||
502 | { | ||
503 | ✗ | data.joint_v = vs.template segment<NV>(idx_v()); | |
504 | ✗ | data.v().noalias() = data.S.angularSubspace() * data.joint_v; | |
505 | |||
506 | // TODO: need to be done | ||
507 | // #define q_dot data.joint_v | ||
508 | // Scalar tmp; | ||
509 | // tmp = (-s1+axis2.x()*axis2.x()*s1)*axis1.x() + | ||
510 | // (axis2.x()*axis2.y()*s1+axis2.z()*c1)*axis1.y() + | ||
511 | // (axis2.x()*axis2.z()*s1-axis2.y()*c1)*axis1.z(); data.c()(0) = tmp * | ||
512 | // q_dot(1)*q_dot(0); tmp = (axis2.x()*axis2.y()*s1-axis2.z()*c1)*axis1.x() + | ||
513 | // (-s1+axis2.y()*axis2.y()*s1)*axis1.y() + | ||
514 | // (axis2.y()*axis2.z()*s1+axis2.x()*c1)*axis1.z(); data.c()(1) = tmp * | ||
515 | // q_dot(1)*q_dot(0); tmp = (axis2.z()*axis2.x()*s1+axis2.y()*c1)*axis1.x() + | ||
516 | // (axis2.y()*axis2.z()*s1-axis2.x()*c1)*axis1.y() + | ||
517 | // (-s1+axis2.z()*axis2.z()*s1)*axis1.z(); data.c()(2) = tmp * q_dot(1)*q_dot(0); | ||
518 | // #undef q_dot | ||
519 | } | ||
520 | |||
521 | template<typename ConfigVector, typename TangentVector> | ||
522 | 6 | void calc( | |
523 | JointDataDerived & data, | ||
524 | const typename Eigen::MatrixBase<ConfigVector> & qs, | ||
525 | const typename Eigen::MatrixBase<TangentVector> & vs) const | ||
526 | { | ||
527 |
3/6✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6 times.
✗ Branch 8 not taken.
|
6 | data.joint_q = qs.template segment<NQ>(idx_q()); |
528 | |||
529 | Scalar c0, s0; | ||
530 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
6 | SINCOS(data.joint_q(0), &s0, &c0); |
531 | Scalar c1, s1; | ||
532 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
6 | SINCOS(data.joint_q(1), &s1, &c1); |
533 | |||
534 |
2/4✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
|
6 | Matrix3 rot1, rot2; |
535 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
6 | toRotationMatrix(axis1, c0, s0, rot1); |
536 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
6 | toRotationMatrix(axis2, c1, s1, rot2); |
537 |
3/6✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6 times.
✗ Branch 8 not taken.
|
6 | data.M.rotation() = rot1 * rot2; |
538 | |||
539 |
4/10✓ Branch 2 taken 6 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 6 times.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✓ Branch 8 taken 6 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 6 times.
✗ Branch 12 not taken.
|
6 | data.S.angularSubspace() << rot2.coeffRef(0, 0) * axis1.x() + rot2.coeffRef(1, 0) * axis1.y() |
540 |
3/8✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✓ Branch 7 taken 6 times.
✗ Branch 8 not taken.
|
6 | + rot2.coeffRef(2, 0) * axis1.z(), |
541 |
2/4✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
|
6 | axis2.x(), |
542 |
4/10✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✓ Branch 7 taken 6 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 6 times.
✗ Branch 11 not taken.
|
6 | rot2.coeffRef(0, 1) * axis1.x() + rot2.coeffRef(1, 1) * axis1.y() |
543 |
3/8✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✓ Branch 7 taken 6 times.
✗ Branch 8 not taken.
|
6 | + rot2.coeffRef(2, 1) * axis1.z(), |
544 |
2/4✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
|
6 | axis2.y(), |
545 |
4/10✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✓ Branch 7 taken 6 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 6 times.
✗ Branch 11 not taken.
|
6 | rot2.coeffRef(0, 2) * axis1.x() + rot2.coeffRef(1, 2) * axis1.y() |
546 |
3/8✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✓ Branch 7 taken 6 times.
✗ Branch 8 not taken.
|
12 | + rot2.coeffRef(2, 2) * axis1.z(), |
547 |
2/4✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
|
6 | axis2.z(); |
548 | |||
549 |
3/6✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6 times.
✗ Branch 8 not taken.
|
6 | data.joint_v = vs.template segment<NV>(idx_v()); |
550 |
3/6✓ Branch 2 taken 6 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 6 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 6 times.
✗ Branch 10 not taken.
|
6 | data.v().noalias() = data.S.angularSubspace() * data.joint_v; |
551 | |||
552 | #define q_dot data.joint_v | ||
553 | Scalar tmp; | ||
554 |
3/6✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6 times.
✗ Branch 8 not taken.
|
6 | tmp = (-s1 + axis2.x() * axis2.x() * s1) * axis1.x() |
555 |
4/8✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 6 times.
✗ Branch 11 not taken.
|
6 | + (axis2.x() * axis2.y() * s1 + axis2.z() * c1) * axis1.y() |
556 |
4/8✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 6 times.
✗ Branch 11 not taken.
|
6 | + (axis2.x() * axis2.z() * s1 - axis2.y() * c1) * axis1.z(); |
557 |
3/6✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 6 times.
✗ Branch 9 not taken.
|
6 | data.c()(0) = tmp * q_dot(1) * q_dot(0); |
558 |
4/8✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 6 times.
✗ Branch 11 not taken.
|
6 | tmp = (axis2.x() * axis2.y() * s1 - axis2.z() * c1) * axis1.x() |
559 |
3/6✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6 times.
✗ Branch 8 not taken.
|
6 | + (-s1 + axis2.y() * axis2.y() * s1) * axis1.y() |
560 |
4/8✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 6 times.
✗ Branch 11 not taken.
|
6 | + (axis2.y() * axis2.z() * s1 + axis2.x() * c1) * axis1.z(); |
561 |
3/6✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 6 times.
✗ Branch 9 not taken.
|
6 | data.c()(1) = tmp * q_dot(1) * q_dot(0); |
562 |
4/8✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 6 times.
✗ Branch 11 not taken.
|
6 | tmp = (axis2.z() * axis2.x() * s1 + axis2.y() * c1) * axis1.x() |
563 |
4/8✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 6 times.
✗ Branch 11 not taken.
|
6 | + (axis2.y() * axis2.z() * s1 - axis2.x() * c1) * axis1.y() |
564 |
3/6✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6 times.
✗ Branch 8 not taken.
|
6 | + (-s1 + axis2.z() * axis2.z() * s1) * axis1.z(); |
565 |
3/6✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 6 times.
✗ Branch 9 not taken.
|
6 | data.c()(2) = tmp * q_dot(1) * q_dot(0); |
566 | #undef q_dot | ||
567 | 6 | } | |
568 | |||
569 | template<typename VectorLike, typename Matrix6Like> | ||
570 | ✗ | void calc_aba( | |
571 | JointDataDerived & data, | ||
572 | const Eigen::MatrixBase<VectorLike> & armature, | ||
573 | const Eigen::MatrixBase<Matrix6Like> & I, | ||
574 | const bool update_I) const | ||
575 | { | ||
576 | ✗ | data.U.noalias() = I.template middleCols<3>(Motion::ANGULAR) * data.S.angularSubspace(); | |
577 | ✗ | data.StU.noalias() = | |
578 | ✗ | data.S.angularSubspace().transpose() * data.U.template middleRows<3>(Motion::ANGULAR); | |
579 | ✗ | data.StU.diagonal() += armature; | |
580 | ✗ | internal::PerformStYSInversion<Scalar>::run(data.StU, data.Dinv); | |
581 | |||
582 | ✗ | data.UDinv.noalias() = data.U * data.Dinv; | |
583 | |||
584 | ✗ | if (update_I) | |
585 | ✗ | PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like, I).noalias() -= data.UDinv * data.U.transpose(); | |
586 | } | ||
587 | |||
588 | 40 | static std::string classname() | |
589 | { | ||
590 |
1/2✓ Branch 2 taken 40 times.
✗ Branch 3 not taken.
|
40 | return std::string("JointModelUniversal"); |
591 | } | ||
592 | ✗ | std::string shortname() const | |
593 | { | ||
594 | ✗ | return classname(); | |
595 | } | ||
596 | |||
597 | /// \returns An expression of *this with the Scalar type casted to NewScalar. | ||
598 | template<typename NewScalar> | ||
599 | ✗ | JointModelUniversalTpl<NewScalar, Options> cast() const | |
600 | { | ||
601 | typedef JointModelUniversalTpl<NewScalar, Options> ReturnType; | ||
602 | ✗ | ReturnType res(axis1.template cast<NewScalar>(), axis2.template cast<NewScalar>()); | |
603 | ✗ | res.setIndexes(id(), idx_q(), idx_v()); | |
604 | ✗ | return res; | |
605 | } | ||
606 | |||
607 | /// \brief 3d main axii of the joint. | ||
608 | /// | ||
609 | Vector3 axis1; | ||
610 | Vector3 axis2; | ||
611 | }; // struct JointModelUniversalTpl | ||
612 | |||
613 | } // namespace pinocchio | ||
614 | |||
615 | #include <boost/type_traits.hpp> | ||
616 | |||
617 | namespace boost | ||
618 | { | ||
619 | template<typename Scalar, int Options> | ||
620 | struct has_nothrow_constructor<::pinocchio::JointModelUniversalTpl<Scalar, Options>> | ||
621 | : public integral_constant<bool, true> | ||
622 | { | ||
623 | }; | ||
624 | |||
625 | template<typename Scalar, int Options> | ||
626 | struct has_nothrow_copy<::pinocchio::JointModelUniversalTpl<Scalar, Options>> | ||
627 | : public integral_constant<bool, true> | ||
628 | { | ||
629 | }; | ||
630 | |||
631 | template<typename Scalar, int Options> | ||
632 | struct has_nothrow_constructor<::pinocchio::JointDataUniversalTpl<Scalar, Options>> | ||
633 | : public integral_constant<bool, true> | ||
634 | { | ||
635 | }; | ||
636 | |||
637 | template<typename Scalar, int Options> | ||
638 | struct has_nothrow_copy<::pinocchio::JointDataUniversalTpl<Scalar, Options>> | ||
639 | : public integral_constant<bool, true> | ||
640 | { | ||
641 | }; | ||
642 | } // namespace boost | ||
643 | |||
644 | #endif // ifndef __pinocchio_multibody_joint_universal_hpp__ | ||
645 |