Directory: | ./ |
---|---|
File: | include/pinocchio/multibody/joint/joint-spherical.hpp |
Date: | 2024-08-27 18:20:05 |
Exec | Total | Coverage | |
---|---|---|---|
Lines: | 69 | 158 | 43.7% |
Branches: | 33 | 228 | 14.5% |
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_spherical_hpp__ | ||
7 | #define __pinocchio_multibody_joint_spherical_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/math/sincos.hpp" | ||
13 | #include "pinocchio/spatial/inertia.hpp" | ||
14 | #include "pinocchio/spatial/skew.hpp" | ||
15 | |||
16 | namespace pinocchio | ||
17 | { | ||
18 | |||
19 | template<typename Scalar, int Options = context::Options> | ||
20 | struct MotionSphericalTpl; | ||
21 | typedef MotionSphericalTpl<context::Scalar> MotionSpherical; | ||
22 | |||
23 | template<typename Scalar, int Options> | ||
24 | struct SE3GroupAction<MotionSphericalTpl<Scalar, Options>> | ||
25 | { | ||
26 | typedef MotionTpl<Scalar, Options> ReturnType; | ||
27 | }; | ||
28 | |||
29 | template<typename Scalar, int Options, typename MotionDerived> | ||
30 | struct MotionAlgebraAction<MotionSphericalTpl<Scalar, Options>, MotionDerived> | ||
31 | { | ||
32 | typedef MotionTpl<Scalar, Options> ReturnType; | ||
33 | }; | ||
34 | |||
35 | template<typename _Scalar, int _Options> | ||
36 | struct traits<MotionSphericalTpl<_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 MotionSphericalTpl | ||
63 | |||
64 | template<typename _Scalar, int _Options> | ||
65 | struct MotionSphericalTpl : MotionBase<MotionSphericalTpl<_Scalar, _Options>> | ||
66 | { | ||
67 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW | ||
68 | |||
69 | MOTION_TYPEDEF_TPL(MotionSphericalTpl); | ||
70 | |||
71 | ✗ | MotionSphericalTpl() | |
72 | { | ||
73 | } | ||
74 | |||
75 | template<typename Vector3Like> | ||
76 | 372 | MotionSphericalTpl(const Eigen::MatrixBase<Vector3Like> & w) | |
77 | 372 | : m_w(w) | |
78 | { | ||
79 | 372 | } | |
80 | |||
81 | ✗ | Vector3 & operator()() | |
82 | { | ||
83 | ✗ | return m_w; | |
84 | } | ||
85 | const Vector3 & operator()() const | ||
86 | { | ||
87 | return m_w; | ||
88 | } | ||
89 | |||
90 | ✗ | inline PlainReturnType plain() const | |
91 | { | ||
92 | ✗ | return PlainReturnType(PlainReturnType::Vector3::Zero(), m_w); | |
93 | } | ||
94 | |||
95 | template<typename MotionDerived> | ||
96 | 3 | void addTo(MotionDense<MotionDerived> & other) const | |
97 | { | ||
98 |
1/2✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
|
3 | other.angular() += m_w; |
99 | 3 | } | |
100 | |||
101 | template<typename Derived> | ||
102 | 127 | void setTo(MotionDense<Derived> & other) const | |
103 | { | ||
104 |
1/2✓ Branch 2 taken 127 times.
✗ Branch 3 not taken.
|
127 | other.linear().setZero(); |
105 |
1/2✓ Branch 2 taken 127 times.
✗ Branch 3 not taken.
|
127 | other.angular() = m_w; |
106 | 127 | } | |
107 | |||
108 | ✗ | MotionSphericalTpl __plus__(const MotionSphericalTpl & other) const | |
109 | { | ||
110 | ✗ | return MotionSphericalTpl(m_w + other.m_w); | |
111 | } | ||
112 | |||
113 | ✗ | bool isEqual_impl(const MotionSphericalTpl & other) const | |
114 | { | ||
115 | ✗ | return internal::comparison_eq(m_w, other.m_w); | |
116 | } | ||
117 | |||
118 | template<typename MotionDerived> | ||
119 | ✗ | bool isEqual_impl(const MotionDense<MotionDerived> & other) const | |
120 | { | ||
121 | ✗ | return internal::comparison_eq(other.angular(), m_w) && other.linear().isZero(0); | |
122 | } | ||
123 | |||
124 | template<typename S2, int O2, typename D2> | ||
125 | ✗ | void se3Action_impl(const SE3Tpl<S2, O2> & m, MotionDense<D2> & v) const | |
126 | { | ||
127 | // Angular | ||
128 | ✗ | v.angular().noalias() = m.rotation() * m_w; | |
129 | |||
130 | // Linear | ||
131 | ✗ | v.linear().noalias() = m.translation().cross(v.angular()); | |
132 | } | ||
133 | |||
134 | template<typename S2, int O2> | ||
135 | ✗ | MotionPlain se3Action_impl(const SE3Tpl<S2, O2> & m) const | |
136 | { | ||
137 | ✗ | MotionPlain res; | |
138 | ✗ | se3Action_impl(m, res); | |
139 | ✗ | return res; | |
140 | } | ||
141 | |||
142 | template<typename S2, int O2, typename D2> | ||
143 | ✗ | void se3ActionInverse_impl(const SE3Tpl<S2, O2> & m, MotionDense<D2> & v) const | |
144 | { | ||
145 | // Linear | ||
146 | // TODO: use v.angular() as temporary variable | ||
147 | ✗ | Vector3 v3_tmp; | |
148 | ✗ | v3_tmp.noalias() = m_w.cross(m.translation()); | |
149 | ✗ | v.linear().noalias() = m.rotation().transpose() * v3_tmp; | |
150 | |||
151 | // Angular | ||
152 | ✗ | v.angular().noalias() = m.rotation().transpose() * m_w; | |
153 | } | ||
154 | |||
155 | template<typename S2, int O2> | ||
156 | ✗ | MotionPlain se3ActionInverse_impl(const SE3Tpl<S2, O2> & m) const | |
157 | { | ||
158 | ✗ | MotionPlain res; | |
159 | ✗ | se3ActionInverse_impl(m, res); | |
160 | ✗ | return res; | |
161 | } | ||
162 | |||
163 | template<typename M1, typename M2> | ||
164 | 115 | void motionAction(const MotionDense<M1> & v, MotionDense<M2> & mout) const | |
165 | { | ||
166 | // Linear | ||
167 |
4/8✓ Branch 2 taken 115 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 115 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 115 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 115 times.
✗ Branch 12 not taken.
|
115 | mout.linear().noalias() = v.linear().cross(m_w); |
168 | |||
169 | // Angular | ||
170 |
4/8✓ Branch 2 taken 115 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 115 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 115 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 115 times.
✗ Branch 12 not taken.
|
115 | mout.angular().noalias() = v.angular().cross(m_w); |
171 | 115 | } | |
172 | |||
173 | template<typename M1> | ||
174 | 115 | MotionPlain motionAction(const MotionDense<M1> & v) const | |
175 | { | ||
176 | 115 | MotionPlain res; | |
177 | 115 | motionAction(v, res); | |
178 | 115 | return res; | |
179 | } | ||
180 | |||
181 | 112 | const Vector3 & angular() const | |
182 | { | ||
183 | 112 | return m_w; | |
184 | } | ||
185 | 127 | Vector3 & angular() | |
186 | { | ||
187 | 127 | return m_w; | |
188 | } | ||
189 | |||
190 | protected: | ||
191 | Vector3 m_w; | ||
192 | }; // struct MotionSphericalTpl | ||
193 | |||
194 | template<typename S1, int O1, typename MotionDerived> | ||
195 | inline typename MotionDerived::MotionPlain | ||
196 | 112 | operator+(const MotionSphericalTpl<S1, O1> & m1, const MotionDense<MotionDerived> & m2) | |
197 | { | ||
198 |
3/6✓ Branch 3 taken 112 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 112 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 112 times.
✗ Branch 10 not taken.
|
224 | return typename MotionDerived::MotionPlain(m2.linear(), m2.angular() + m1.angular()); |
199 | } | ||
200 | |||
201 | template<typename Scalar, int Options> | ||
202 | struct JointMotionSubspaceSphericalTpl; | ||
203 | |||
204 | template<typename _Scalar, int _Options> | ||
205 | struct traits<JointMotionSubspaceSphericalTpl<_Scalar, _Options>> | ||
206 | { | ||
207 | typedef _Scalar Scalar; | ||
208 | enum | ||
209 | { | ||
210 | Options = _Options | ||
211 | }; | ||
212 | enum | ||
213 | { | ||
214 | LINEAR = 0, | ||
215 | ANGULAR = 3 | ||
216 | }; | ||
217 | typedef MotionSphericalTpl<Scalar, Options> JointMotion; | ||
218 | typedef Eigen::Matrix<Scalar, 3, 1, Options> JointForce; | ||
219 | typedef Eigen::Matrix<Scalar, 6, 3, Options> DenseBase; | ||
220 | typedef Eigen::Matrix<Scalar, 3, 3, Options> ReducedSquaredMatrix; | ||
221 | |||
222 | typedef DenseBase MatrixReturnType; | ||
223 | typedef const DenseBase ConstMatrixReturnType; | ||
224 | |||
225 | typedef typename ReducedSquaredMatrix::IdentityReturnType StDiagonalMatrixSOperationReturnType; | ||
226 | }; // struct traits struct JointMotionSubspaceSphericalTpl | ||
227 | |||
228 | template<typename _Scalar, int _Options> | ||
229 | struct JointMotionSubspaceSphericalTpl | ||
230 | : public JointMotionSubspaceBase<JointMotionSubspaceSphericalTpl<_Scalar, _Options>> | ||
231 | { | ||
232 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW | ||
233 | |||
234 | PINOCCHIO_CONSTRAINT_TYPEDEF_TPL(JointMotionSubspaceSphericalTpl) | ||
235 | |||
236 | 40 | JointMotionSubspaceSphericalTpl() | |
237 | { | ||
238 | 40 | } | |
239 | |||
240 | enum | ||
241 | { | ||
242 | NV = 3 | ||
243 | }; | ||
244 | |||
245 | ✗ | int nv_impl() const | |
246 | { | ||
247 | ✗ | return NV; | |
248 | } | ||
249 | |||
250 | template<typename Vector3Like> | ||
251 | 115 | JointMotion __mult__(const Eigen::MatrixBase<Vector3Like> & w) const | |
252 | { | ||
253 | EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3Like, 3); | ||
254 | 115 | return JointMotion(w); | |
255 | } | ||
256 | |||
257 | struct TransposeConst : JointMotionSubspaceTransposeBase<JointMotionSubspaceSphericalTpl> | ||
258 | { | ||
259 | template<typename Derived> | ||
260 | 3 | typename ForceDense<Derived>::ConstAngularType operator*(const ForceDense<Derived> & phi) | |
261 | { | ||
262 | 3 | return phi.angular(); | |
263 | } | ||
264 | |||
265 | /* [CRBA] MatrixBase operator* (Constraint::Transpose S, ForceSet::Block) */ | ||
266 | template<typename MatrixDerived> | ||
267 | const typename SizeDepType<3>::RowsReturn<MatrixDerived>::ConstType | ||
268 | ✗ | operator*(const Eigen::MatrixBase<MatrixDerived> & F) const | |
269 | { | ||
270 | ✗ | assert(F.rows() == 6); | |
271 | ✗ | return F.derived().template middleRows<3>(Inertia::ANGULAR); | |
272 | } | ||
273 | }; | ||
274 | |||
275 | ✗ | TransposeConst transpose() const | |
276 | { | ||
277 | ✗ | return TransposeConst(); | |
278 | } | ||
279 | |||
280 | ✗ | DenseBase matrix_impl() const | |
281 | { | ||
282 | ✗ | DenseBase S; | |
283 | ✗ | S.template block<3, 3>(LINEAR, 0).setZero(); | |
284 | ✗ | S.template block<3, 3>(ANGULAR, 0).setIdentity(); | |
285 | ✗ | return S; | |
286 | } | ||
287 | |||
288 | template<typename S1, int O1> | ||
289 | 4 | Eigen::Matrix<S1, 6, 3, O1> se3Action(const SE3Tpl<S1, O1> & m) const | |
290 | { | ||
291 | 4 | Eigen::Matrix<S1, 6, 3, O1> X_subspace; | |
292 |
3/6✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 4 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 4 times.
✗ Branch 9 not taken.
|
4 | cross(m.translation(), m.rotation(), X_subspace.template middleRows<3>(LINEAR)); |
293 |
1/2✓ Branch 3 taken 4 times.
✗ Branch 4 not taken.
|
4 | X_subspace.template middleRows<3>(ANGULAR) = m.rotation(); |
294 | |||
295 | 4 | return X_subspace; | |
296 | } | ||
297 | |||
298 | template<typename S1, int O1> | ||
299 | ✗ | Eigen::Matrix<S1, 6, 3, O1> se3ActionInverse(const SE3Tpl<S1, O1> & m) const | |
300 | { | ||
301 | ✗ | Eigen::Matrix<S1, 6, 3, O1> X_subspace; | |
302 | ✗ | XAxis::cross(m.translation(), X_subspace.template middleRows<3>(ANGULAR).col(0)); | |
303 | ✗ | YAxis::cross(m.translation(), X_subspace.template middleRows<3>(ANGULAR).col(1)); | |
304 | ✗ | ZAxis::cross(m.translation(), X_subspace.template middleRows<3>(ANGULAR).col(2)); | |
305 | |||
306 | ✗ | X_subspace.template middleRows<3>(LINEAR).noalias() = | |
307 | ✗ | m.rotation().transpose() * X_subspace.template middleRows<3>(ANGULAR); | |
308 | ✗ | X_subspace.template middleRows<3>(ANGULAR) = m.rotation().transpose(); | |
309 | |||
310 | ✗ | return X_subspace; | |
311 | } | ||
312 | |||
313 | template<typename MotionDerived> | ||
314 | ✗ | DenseBase motionAction(const MotionDense<MotionDerived> & m) const | |
315 | { | ||
316 | ✗ | const typename MotionDerived::ConstLinearType v = m.linear(); | |
317 | ✗ | const typename MotionDerived::ConstAngularType w = m.angular(); | |
318 | |||
319 | ✗ | DenseBase res; | |
320 | ✗ | skew(v, res.template middleRows<3>(LINEAR)); | |
321 | ✗ | skew(w, res.template middleRows<3>(ANGULAR)); | |
322 | |||
323 | ✗ | return res; | |
324 | } | ||
325 | |||
326 | ✗ | bool isEqual(const JointMotionSubspaceSphericalTpl &) const | |
327 | { | ||
328 | ✗ | return true; | |
329 | } | ||
330 | |||
331 | }; // struct JointMotionSubspaceSphericalTpl | ||
332 | |||
333 | template<typename MotionDerived, typename S2, int O2> | ||
334 | inline typename MotionDerived::MotionPlain | ||
335 | 115 | operator^(const MotionDense<MotionDerived> & m1, const MotionSphericalTpl<S2, O2> & m2) | |
336 | { | ||
337 | 115 | return m2.motionAction(m1); | |
338 | } | ||
339 | |||
340 | /* [CRBA] ForceSet operator* (Inertia Y,Constraint S) */ | ||
341 | template<typename S1, int O1, typename S2, int O2> | ||
342 | inline Eigen::Matrix<S2, 6, 3, O2> | ||
343 | ✗ | operator*(const InertiaTpl<S1, O1> & Y, const JointMotionSubspaceSphericalTpl<S2, O2> &) | |
344 | { | ||
345 | typedef InertiaTpl<S1, O1> Inertia; | ||
346 | typedef typename Inertia::Symmetric3 Symmetric3; | ||
347 | ✗ | Eigen::Matrix<S2, 6, 3, O2> M; | |
348 | // M.block <3,3> (Inertia::LINEAR, 0) = - Y.mass () * skew(Y.lever ()); | ||
349 | ✗ | M.template block<3, 3>(Inertia::LINEAR, 0) = alphaSkew(-Y.mass(), Y.lever()); | |
350 | ✗ | M.template block<3, 3>(Inertia::ANGULAR, 0) = | |
351 | ✗ | (Y.inertia() - typename Symmetric3::AlphaSkewSquare(Y.mass(), Y.lever())).matrix(); | |
352 | ✗ | return M; | |
353 | } | ||
354 | |||
355 | /* [ABA] Y*S operator*/ | ||
356 | template<typename M6Like, typename S2, int O2> | ||
357 | inline typename SizeDepType<3>::ColsReturn<M6Like>::ConstType | ||
358 | ✗ | operator*(const Eigen::MatrixBase<M6Like> & Y, const JointMotionSubspaceSphericalTpl<S2, O2> &) | |
359 | { | ||
360 | typedef JointMotionSubspaceSphericalTpl<S2, O2> Constraint; | ||
361 | ✗ | return Y.derived().template middleCols<3>(Constraint::ANGULAR); | |
362 | } | ||
363 | |||
364 | template<typename S1, int O1> | ||
365 | struct SE3GroupAction<JointMotionSubspaceSphericalTpl<S1, O1>> | ||
366 | { | ||
367 | typedef Eigen::Matrix<S1, 6, 3, O1> ReturnType; | ||
368 | }; | ||
369 | |||
370 | template<typename S1, int O1, typename MotionDerived> | ||
371 | struct MotionAlgebraAction<JointMotionSubspaceSphericalTpl<S1, O1>, MotionDerived> | ||
372 | { | ||
373 | typedef Eigen::Matrix<S1, 6, 3, O1> ReturnType; | ||
374 | }; | ||
375 | |||
376 | template<typename Scalar, int Options> | ||
377 | struct JointSphericalTpl; | ||
378 | |||
379 | template<typename _Scalar, int _Options> | ||
380 | struct traits<JointSphericalTpl<_Scalar, _Options>> | ||
381 | { | ||
382 | enum | ||
383 | { | ||
384 | NQ = 4, | ||
385 | NV = 3 | ||
386 | }; | ||
387 | typedef _Scalar Scalar; | ||
388 | enum | ||
389 | { | ||
390 | Options = _Options | ||
391 | }; | ||
392 | typedef JointDataSphericalTpl<Scalar, Options> JointDataDerived; | ||
393 | typedef JointModelSphericalTpl<Scalar, Options> JointModelDerived; | ||
394 | typedef JointMotionSubspaceSphericalTpl<Scalar, Options> Constraint_t; | ||
395 | typedef SE3Tpl<Scalar, Options> Transformation_t; | ||
396 | typedef MotionSphericalTpl<Scalar, Options> Motion_t; | ||
397 | typedef MotionZeroTpl<Scalar, Options> Bias_t; | ||
398 | |||
399 | // [ABA] | ||
400 | typedef Eigen::Matrix<Scalar, 6, NV, Options> U_t; | ||
401 | typedef Eigen::Matrix<Scalar, NV, NV, Options> D_t; | ||
402 | typedef Eigen::Matrix<Scalar, 6, NV, Options> UD_t; | ||
403 | |||
404 | typedef Eigen::Matrix<Scalar, NQ, 1, Options> ConfigVector_t; | ||
405 | typedef Eigen::Matrix<Scalar, NV, 1, Options> TangentVector_t; | ||
406 | |||
407 | PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE | ||
408 | }; | ||
409 | |||
410 | template<typename _Scalar, int _Options> | ||
411 | struct traits<JointDataSphericalTpl<_Scalar, _Options>> | ||
412 | { | ||
413 | typedef JointSphericalTpl<_Scalar, _Options> JointDerived; | ||
414 | typedef _Scalar Scalar; | ||
415 | }; | ||
416 | |||
417 | template<typename _Scalar, int _Options> | ||
418 | struct traits<JointModelSphericalTpl<_Scalar, _Options>> | ||
419 | { | ||
420 | typedef JointSphericalTpl<_Scalar, _Options> JointDerived; | ||
421 | typedef _Scalar Scalar; | ||
422 | }; | ||
423 | |||
424 | template<typename _Scalar, int _Options> | ||
425 | struct JointDataSphericalTpl : public JointDataBase<JointDataSphericalTpl<_Scalar, _Options>> | ||
426 | { | ||
427 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW | ||
428 | |||
429 | typedef JointSphericalTpl<_Scalar, _Options> JointDerived; | ||
430 | PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(JointDerived); | ||
431 | 1163 | PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR | |
432 | |||
433 | ConfigVector_t joint_q; | ||
434 | TangentVector_t joint_v; | ||
435 | |||
436 | Constraint_t S; | ||
437 | Transformation_t M; | ||
438 | Motion_t v; | ||
439 | Bias_t c; | ||
440 | |||
441 | // [ABA] specific data | ||
442 | U_t U; | ||
443 | D_t Dinv; | ||
444 | UD_t UDinv; | ||
445 | D_t StU; | ||
446 | |||
447 | 40 | JointDataSphericalTpl() | |
448 |
1/2✓ Branch 1 taken 40 times.
✗ Branch 2 not taken.
|
40 | : joint_q(Scalar(0), Scalar(0), Scalar(0), Scalar(1)) |
449 |
1/2✓ Branch 2 taken 40 times.
✗ Branch 3 not taken.
|
40 | , joint_v(TangentVector_t::Zero()) |
450 | 40 | , M(Transformation_t::Identity()) | |
451 |
1/2✓ Branch 2 taken 40 times.
✗ Branch 3 not taken.
|
40 | , v(Motion_t::Vector3::Zero()) |
452 |
1/2✓ Branch 2 taken 40 times.
✗ Branch 3 not taken.
|
40 | , U(U_t::Zero()) |
453 |
1/2✓ Branch 2 taken 40 times.
✗ Branch 3 not taken.
|
40 | , Dinv(D_t::Zero()) |
454 |
1/2✓ Branch 2 taken 40 times.
✗ Branch 3 not taken.
|
40 | , UDinv(UD_t::Zero()) |
455 |
1/2✓ Branch 4 taken 40 times.
✗ Branch 5 not taken.
|
120 | , StU(D_t::Zero()) |
456 | { | ||
457 | 40 | } | |
458 | |||
459 | 40 | static std::string classname() | |
460 | { | ||
461 |
1/2✓ Branch 2 taken 40 times.
✗ Branch 3 not taken.
|
40 | return std::string("JointDataSpherical"); |
462 | } | ||
463 | ✗ | std::string shortname() const | |
464 | { | ||
465 | ✗ | return classname(); | |
466 | } | ||
467 | |||
468 | }; // struct JointDataSphericalTpl | ||
469 | |||
470 | PINOCCHIO_JOINT_CAST_TYPE_SPECIALIZATION(JointModelSphericalTpl); | ||
471 | template<typename _Scalar, int _Options> | ||
472 | struct JointModelSphericalTpl : public JointModelBase<JointModelSphericalTpl<_Scalar, _Options>> | ||
473 | { | ||
474 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW | ||
475 | |||
476 | typedef JointSphericalTpl<_Scalar, _Options> JointDerived; | ||
477 | PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived); | ||
478 | |||
479 | typedef JointModelBase<JointModelSphericalTpl> Base; | ||
480 | using Base::id; | ||
481 | using Base::idx_q; | ||
482 | using Base::idx_v; | ||
483 | using Base::setIndexes; | ||
484 | |||
485 | 20 | JointDataDerived createData() const | |
486 | { | ||
487 | 20 | return JointDataDerived(); | |
488 | } | ||
489 | |||
490 | ✗ | const std::vector<bool> hasConfigurationLimit() const | |
491 | { | ||
492 | ✗ | return {false, false, false, false}; | |
493 | } | ||
494 | |||
495 | ✗ | const std::vector<bool> hasConfigurationLimitInTangent() const | |
496 | { | ||
497 | ✗ | return {false, false, false}; | |
498 | } | ||
499 | |||
500 | template<typename ConfigVectorLike> | ||
501 | inline void forwardKinematics( | ||
502 | Transformation_t & M, const Eigen::MatrixBase<ConfigVectorLike> & q_joint) const | ||
503 | { | ||
504 | typedef typename ConfigVectorLike::Scalar Scalar; | ||
505 | EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigVector_t, ConfigVectorLike); | ||
506 | typedef | ||
507 | typename Eigen::Quaternion<Scalar, PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorLike)::Options> | ||
508 | Quaternion; | ||
509 | typedef Eigen::Map<const Quaternion> ConstQuaternionMap; | ||
510 | |||
511 | ConstQuaternionMap quat(q_joint.derived().data()); | ||
512 | // assert(math::fabs(quat.coeffs().squaredNorm()-1.) <= sqrt(Eigen::NumTraits<typename | ||
513 | // V::Scalar>::epsilon())); TODO: check validity of the rhs precision | ||
514 | assert(math::fabs(static_cast<Scalar>(quat.coeffs().squaredNorm() - 1)) <= 1e-4); | ||
515 | |||
516 | M.rotation(quat.matrix()); | ||
517 | M.translation().setZero(); | ||
518 | } | ||
519 | |||
520 | template<typename QuaternionDerived> | ||
521 | 111 | void calc( | |
522 | JointDataDerived & data, const typename Eigen::QuaternionBase<QuaternionDerived> & quat) const | ||
523 | { | ||
524 | 111 | data.joint_q = quat.coeffs(); | |
525 |
1/2✓ Branch 2 taken 111 times.
✗ Branch 3 not taken.
|
111 | data.M.rotation(quat.matrix()); |
526 | 111 | } | |
527 | |||
528 | template<typename ConfigVector> | ||
529 | EIGEN_DONT_INLINE void | ||
530 | ✗ | calc(JointDataDerived & data, const typename Eigen::PlainObjectBase<ConfigVector> & qs) const | |
531 | { | ||
532 | typedef typename Eigen::Quaternion<typename ConfigVector::Scalar, ConfigVector::Options> | ||
533 | Quaternion; | ||
534 | typedef Eigen::Map<const Quaternion> ConstQuaternionMap; | ||
535 | |||
536 | ✗ | ConstQuaternionMap quat(qs.template segment<NQ>(idx_q()).data()); | |
537 | ✗ | calc(data, quat); | |
538 | } | ||
539 | |||
540 | template<typename ConfigVector> | ||
541 | EIGEN_DONT_INLINE void | ||
542 | 111 | calc(JointDataDerived & data, const typename Eigen::MatrixBase<ConfigVector> & qs) const | |
543 | { | ||
544 | typedef typename Eigen::Quaternion<Scalar, Options> Quaternion; | ||
545 | |||
546 |
3/6✓ Branch 1 taken 111 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 111 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 111 times.
✗ Branch 8 not taken.
|
111 | const Quaternion quat(qs.template segment<NQ>(idx_q())); |
547 |
1/2✓ Branch 1 taken 111 times.
✗ Branch 2 not taken.
|
111 | calc(data, quat); |
548 | 111 | } | |
549 | |||
550 | template<typename TangentVector> | ||
551 | void | ||
552 | ✗ | calc(JointDataDerived & data, const Blank, const typename Eigen::MatrixBase<TangentVector> & vs) | |
553 | const | ||
554 | { | ||
555 | ✗ | data.joint_v = vs.template segment<NV>(idx_v()); | |
556 | ✗ | data.v.angular() = data.joint_v; | |
557 | } | ||
558 | |||
559 | template<typename ConfigVector, typename TangentVector> | ||
560 | 130 | void calc( | |
561 | JointDataDerived & data, | ||
562 | const typename Eigen::MatrixBase<ConfigVector> & qs, | ||
563 | const typename Eigen::MatrixBase<TangentVector> & vs) const | ||
564 | { | ||
565 | 130 | calc(data, qs.derived()); | |
566 |
1/2✓ Branch 3 taken 130 times.
✗ Branch 4 not taken.
|
130 | data.joint_v = vs.template segment<NV>(idx_v()); |
567 | 130 | data.v.angular() = data.joint_v; | |
568 | 130 | } | |
569 | |||
570 | template<typename VectorLike, typename Matrix6Like> | ||
571 | ✗ | void calc_aba( | |
572 | JointDataDerived & data, | ||
573 | const Eigen::MatrixBase<VectorLike> & armature, | ||
574 | const Eigen::MatrixBase<Matrix6Like> & I, | ||
575 | const bool update_I) const | ||
576 | { | ||
577 | ✗ | data.U = I.template block<6, 3>(0, Inertia::ANGULAR); | |
578 | ✗ | data.StU = data.U.template middleRows<3>(Inertia::ANGULAR); | |
579 | ✗ | data.StU.diagonal() += armature; | |
580 | |||
581 | ✗ | internal::PerformStYSInversion<Scalar>::run(data.StU, data.Dinv); | |
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 | 42 | static std::string classname() | |
589 | { | ||
590 |
1/2✓ Branch 2 taken 42 times.
✗ Branch 3 not taken.
|
42 | return std::string("JointModelSpherical"); |
591 | } | ||
592 | 2 | std::string shortname() const | |
593 | { | ||
594 | 2 | return classname(); | |
595 | } | ||
596 | |||
597 | /// \returns An expression of *this with the Scalar type casted to NewScalar. | ||
598 | template<typename NewScalar> | ||
599 | ✗ | JointModelSphericalTpl<NewScalar, Options> cast() const | |
600 | { | ||
601 | typedef JointModelSphericalTpl<NewScalar, Options> ReturnType; | ||
602 | ✗ | ReturnType res; | |
603 | ✗ | res.setIndexes(id(), idx_q(), idx_v()); | |
604 | ✗ | return res; | |
605 | } | ||
606 | |||
607 | }; // struct JointModelSphericalTpl | ||
608 | |||
609 | } // namespace pinocchio | ||
610 | |||
611 | #include <boost/type_traits.hpp> | ||
612 | |||
613 | namespace boost | ||
614 | { | ||
615 | template<typename Scalar, int Options> | ||
616 | struct has_nothrow_constructor<::pinocchio::JointModelSphericalTpl<Scalar, Options>> | ||
617 | : public integral_constant<bool, true> | ||
618 | { | ||
619 | }; | ||
620 | |||
621 | template<typename Scalar, int Options> | ||
622 | struct has_nothrow_copy<::pinocchio::JointModelSphericalTpl<Scalar, Options>> | ||
623 | : public integral_constant<bool, true> | ||
624 | { | ||
625 | }; | ||
626 | |||
627 | template<typename Scalar, int Options> | ||
628 | struct has_nothrow_constructor<::pinocchio::JointDataSphericalTpl<Scalar, Options>> | ||
629 | : public integral_constant<bool, true> | ||
630 | { | ||
631 | }; | ||
632 | |||
633 | template<typename Scalar, int Options> | ||
634 | struct has_nothrow_copy<::pinocchio::JointDataSphericalTpl<Scalar, Options>> | ||
635 | : public integral_constant<bool, true> | ||
636 | { | ||
637 | }; | ||
638 | } // namespace boost | ||
639 | |||
640 | #endif // ifndef __pinocchio_multibody_joint_spherical_hpp__ | ||
641 |