GCC Code Coverage Report

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