GCC Code Coverage Report

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