GCC Code Coverage Report

Directory: ./
File: include/pinocchio/multibody/joint/joint-planar.hpp
Date: 2025-02-12 21:03:38
Exec Total Coverage
Lines: 215 221 97.3%
Branches: 246 676 36.4%
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 7 MotionPlanarTpl()
73 7 {
74 7 }
75
76 1 MotionPlanarTpl(const Scalar & x_dot, const Scalar & y_dot, const Scalar & theta_dot)
77 1 {
78
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 m_data << x_dot, y_dot, theta_dot;
79 1 }
80
81 template<typename Vector3Like>
82 3184 MotionPlanarTpl(const Eigen::MatrixBase<Vector3Like> & vj)
83 3184 : m_data(vj)
84 {
85 EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3Like, 3);
86 3184 }
87
88 1033 inline PlainReturnType plain() const
89 {
90 return PlainReturnType(
91
4/8
✓ Branch 1 taken 1031 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1031 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1031 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
 1035 typename PlainReturnType::Vector3(vx(), vy(), Scalar(0)),
92
5/8
✓ Branch 2 taken 1030 times.
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
✓ Branch 5 taken 1030 times.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
 3099 typename PlainReturnType::Vector3(Scalar(0), Scalar(0), wz()));
93 }
94
95 template<typename Derived>
96 1 void addTo(MotionDense<Derived> & other) const
97 {
98
1/4
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
 1 other.linear()[0] += vx();
99
1/4
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
 1 other.linear()[1] += vy();
100
1/4
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
 1 other.angular()[2] += wz();
101 1 }
102
103 template<typename MotionDerived>
104 5 void setTo(MotionDense<MotionDerived> & other) const
105 {
106
5/12
✓ Branch 2 taken 5 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 5 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 5 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 5 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 5 times.
✗ Branch 15 not taken.
✗ Branch 17 not taken.
✗ Branch 18 not taken.
 5 other.linear() << vx(), vy(), (Scalar)0;
107
4/12
✓ Branch 2 taken 5 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 5 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 5 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 5 times.
✗ Branch 12 not taken.
✗ Branch 14 not taken.
✗ Branch 15 not taken.
✗ Branch 17 not taken.
✗ Branch 18 not taken.
 5 other.angular() << (Scalar)0, (Scalar)0, wz();
108 5 }
109
110 template<typename S2, int O2, typename D2>
111 2 void se3Action_impl(const SE3Tpl<S2, O2> & m, MotionDense<D2> & v) const
112 {
113
4/8
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 2 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 2 times.
✗ Branch 14 not taken.
 2 v.angular().noalias() = m.rotation().col(2) * wz();
114
6/12
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 2 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 2 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 2 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 2 times.
✗ Branch 18 not taken.
 2 v.linear().noalias() = m.rotation().template leftCols<2>() * m_data.template head<2>();
115
3/6
✓ Branch 3 taken 2 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 2 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 2 times.
✗ Branch 10 not taken.
 2 v.linear() += m.translation().cross(v.angular());
116 2 }
117
118 template<typename S2, int O2>
119 2 MotionPlain se3Action_impl(const SE3Tpl<S2, O2> & m) const
120 {
121 2 MotionPlain res;
122
0/2
✗ Branch 1 not taken.
✗ Branch 2 not taken.
 2 se3Action_impl(m, res);
123 2 return res;
124 }
125
126 template<typename S2, int O2, typename D2>
127 1 void se3ActionInverse_impl(const SE3Tpl<S2, O2> & m, MotionDense<D2> & v) const
128 {
129 // Linear
130 // TODO: use v.angular() as temporary variable
131
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 Vector3 v3_tmp;
132
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
 1 ZAxis::alphaCross(wz(), m.translation(), v3_tmp);
133
2/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
 1 v3_tmp[0] += vx();
134
2/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
 1 v3_tmp[1] += vy();
135
6/12
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
 1 v.linear().noalias() = m.rotation().transpose() * v3_tmp;
136
137 // Angular
138
8/16
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 1 times.
✗ Branch 23 not taken.
 1 v.angular().noalias() = m.rotation().transpose().col(2) * wz();
139 1 }
140
141 template<typename S2, int O2>
142 1 MotionPlain se3ActionInverse_impl(const SE3Tpl<S2, O2> & m) const
143 {
144 1 MotionPlain res;
145
0/2
✗ Branch 1 not taken.
✗ Branch 2 not taken.
 1 se3ActionInverse_impl(m, res);
146 1 return res;
147 }
148
149 template<typename M1, typename M2>
150 3 void motionAction(const MotionDense<M1> & v, MotionDense<M2> & mout) const
151 {
152 // Linear
153
4/10
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 3 times.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
 3 ZAxis::alphaCross(-wz(), v.linear(), mout.linear());
154
155
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 typename M1::ConstAngularType w_in = v.angular();
156
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 typename M2::LinearType v_out = mout.linear();
157
158
3/10
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3 times.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
 3 v_out[0] -= w_in[2] * vy();
159
3/10
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3 times.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
 3 v_out[1] += w_in[2] * vx();
160
5/20
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 3 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 3 times.
✗ Branch 14 not taken.
✗ Branch 16 not taken.
✗ Branch 17 not taken.
✗ Branch 19 not taken.
✗ Branch 20 not taken.
✗ Branch 22 not taken.
✗ Branch 23 not taken.
✗ Branch 25 not taken.
✗ Branch 26 not taken.
✗ Branch 28 not taken.
✗ Branch 29 not taken.
 3 v_out[2] += -w_in[1] * vx() + w_in[0] * vy();
161
162 // Angular
163
4/10
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 3 times.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
 3 ZAxis::alphaCross(-wz(), v.angular(), mout.angular());
164 3 }
165
166 template<typename M1>
167 3 MotionPlain motionAction(const MotionDense<M1> & v) const
168 {
169 3 MotionPlain res;
170
0/2
✗ Branch 1 not taken.
✗ Branch 2 not taken.
 3 motionAction(v, res);
171 3 return res;
172 }
173
174 1046 const Scalar & vx() const
175 {
176 1046 return m_data[0];
177 }
178 1038 Scalar & vx()
179 {
180 1040 return m_data[0];
181 }
182
183 1046 const Scalar & vy() const
184 {
185 1046 return m_data[1];
186 }
187 1038 Scalar & vy()
188 {
189 1040 return m_data[1];
190 }
191
192 1049 const Scalar & wz() const
193 {
194 1049 return m_data[2];
195 }
196 1038 Scalar & wz()
197 {
198 1040 return m_data[2];
199 }
200
201 const Vector3 & data() const
202 {
203 return m_data;
204 }
205 28 Vector3 & data()
206 {
207 28 return m_data;
208 }
209
210 17 bool isEqual_impl(const MotionPlanarTpl & other) const
211 {
212 17 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 3185 JointMotionSubspacePlanarTpl() {};
273
274 template<typename Vector3Like>
275 3 JointMotion __mult__(const Eigen::MatrixBase<Vector3Like> & vj) const
276 {
277 EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3Like, 3);
278 3 return JointMotion(vj);
279 }
280
281 24 int nv_impl() const
282 {
283 24 return NV;
284 }
285
286 struct ConstraintTranspose : JointMotionSubspaceTransposeBase<JointMotionSubspacePlanarTpl>
287 {
288 const JointMotionSubspacePlanarTpl & ref;
289 5 ConstraintTranspose(const JointMotionSubspacePlanarTpl & ref)
290 5 : ref(ref)
291 {
292 5 }
293
294 template<typename Derived>
295 3 typename ForceDense<Derived>::Vector3 operator*(const ForceDense<Derived> & phi)
296 {
297 typedef typename ForceDense<Derived>::Vector3 Vector3;
298
6/12
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 3 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 3 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 3 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 3 times.
✗ Branch 18 not taken.
 6 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 1 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
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 1 times.
 1 assert(F.rows() == 6);
310
311
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 MatrixReturnType result(3, F.cols());
312
3/8
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
 1 result.template topRows<2>() = F.template topRows<2>();
313
3/8
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
 1 result.template bottomRows<1>() = F.template bottomRows<1>();
314 1 return result;
315 }
316
317 }; // struct ConstraintTranspose
318
319 5 ConstraintTranspose transpose() const
320 {
321 5 return ConstraintTranspose(*this);
322 }
323
324 15 DenseBase matrix_impl() const
325 {
326 15 DenseBase S;
327
1/5
✗ Branch 1 not taken.
✓ Branch 2 taken 14 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
 15 S.template block<3, 3>(Inertia::LINEAR, 0).setZero();
328
1/5
✗ Branch 1 not taken.
✓ Branch 2 taken 14 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
 15 S.template block<3, 3>(Inertia::ANGULAR, 0).setZero();
329
0/6
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
 15 S(Inertia::LINEAR + 0, 0) = Scalar(1);
330
0/6
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
 15 S(Inertia::LINEAR + 1, 1) = Scalar(1);
331
0/6
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
 15 S(Inertia::ANGULAR + 2, 2) = Scalar(1);
332 15 return S;
333 }
334
335 template<typename S1, int O1>
336 7 DenseBase se3Action(const SE3Tpl<S1, O1> & m) const
337 {
338 7 DenseBase X_subspace;
339
340 // LINEAR
341
2/10
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✓ Branch 3 taken 7 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✓ Branch 6 taken 7 times.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
 7 X_subspace.template block<3, 2>(Motion::LINEAR, 0) = m.rotation().template leftCols<2>();
342
4/8
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 7 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 7 times.
✗ Branch 11 not taken.
 7 X_subspace.template block<3, 1>(Motion::LINEAR, 2).noalias() =
343
0/6
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
 7 m.translation().cross(m.rotation().template rightCols<1>());
344
345 // ANGULAR
346
1/5
✗ Branch 1 not taken.
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
 7 X_subspace.template block<3, 2>(Motion::ANGULAR, 0).setZero();
347
2/10
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✓ Branch 3 taken 7 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✓ Branch 6 taken 7 times.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
 7 X_subspace.template block<3, 1>(Motion::ANGULAR, 2) = m.rotation().template rightCols<1>();
348
349 7 return X_subspace;
350 }
351
352 template<typename S1, int O1>
353 3 DenseBase se3ActionInverse(const SE3Tpl<S1, O1> & m) const
354 {
355 3 DenseBase X_subspace;
356
357 // LINEAR
358
2/4
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
 3 X_subspace.template block<3, 2>(Motion::LINEAR, 0) =
359
1/7
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✓ Branch 3 taken 3 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
 3 m.rotation().transpose().template leftCols<2>();
360
3/6
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3 times.
✗ Branch 8 not taken.
 3 X_subspace.template block<3, 1>(Motion::ANGULAR, 2).noalias() =
361
1/8
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
 3 m.rotation().transpose() * m.translation(); // tmp variable
362
5/10
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 3 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 3 times.
✗ Branch 14 not taken.
 3 X_subspace.template block<3, 1>(Motion::LINEAR, 2).noalias() =
363
0/2
✗ Branch 1 not taken.
✗ Branch 2 not taken.
 3 -X_subspace.template block<3, 1>(Motion::ANGULAR, 2)
364
3/6
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3 times.
✗ Branch 8 not taken.
 3 .cross(m.rotation().transpose().template rightCols<1>());
365
366 // ANGULAR
367
1/5
✗ Branch 1 not taken.
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
 3 X_subspace.template block<3, 2>(Motion::ANGULAR, 0).setZero();
368
2/4
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
 3 X_subspace.template block<3, 1>(Motion::ANGULAR, 2) =
369
1/7
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✓ Branch 3 taken 3 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
 3 m.rotation().transpose().template rightCols<1>();
370
371 3 return X_subspace;
372 }
373
374 template<typename MotionDerived>
375 2 DenseBase motionAction(const MotionDense<MotionDerived> & m) const
376 {
377
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 const typename MotionDerived::ConstLinearType v = m.linear();
378
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 const typename MotionDerived::ConstAngularType w = m.angular();
379
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 2 DenseBase res(DenseBase::Zero());
380
381
2/8
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
 2 res(0, 1) = -w[2];
382
2/6
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
 2 res(0, 2) = v[1];
383
2/6
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
 2 res(1, 0) = w[2];
384
2/8
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
 2 res(1, 2) = -v[0];
385
2/8
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
 2 res(2, 0) = -w[1];
386
2/6
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
 2 res(2, 1) = w[0];
387
2/6
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
 2 res(3, 2) = w[1];
388
2/8
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
 2 res(4, 2) = -w[0];
389
390 4 return res;
391 }
392
393 17 bool isEqual(const JointMotionSubspacePlanarTpl &) const
394 {
395 17 return true;
396 }
397
398 }; // struct JointMotionSubspacePlanarTpl
399
400 template<typename MotionDerived, typename S2, int O2>
401 inline typename MotionDerived::MotionPlain
402 2 operator^(const MotionDense<MotionDerived> & m1, const MotionPlanarTpl<S2, O2> & m2)
403 {
404 2 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 2 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
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 ReturnType M;
417
0/2
✗ Branch 1 not taken.
✗ Branch 2 not taken.
 2 const Scalar mass = Y.mass();
418 2 const typename Inertia::Vector3 & com = Y.lever();
419 2 const typename Inertia::Symmetric3 & inertia = Y.inertia();
420
421
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 2 M.template topLeftCorner<3, 3>().setZero();
422
3/6
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
 2 M.template topLeftCorner<2, 2>().diagonal().fill(mass);
423
424
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 2 const typename Inertia::Vector3 mc(mass * com);
425
6/14
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 2 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 2 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 2 times.
✗ Branch 17 not taken.
✗ Branch 19 not taken.
✗ Branch 20 not taken.
 2 M.template rightCols<1>().template head<2>() << -mc(1), mc(0);
426
427
11/30
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 2 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 2 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 2 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 2 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 2 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 2 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 2 times.
✗ Branch 29 not taken.
✓ Branch 31 taken 2 times.
✗ Branch 32 not taken.
✗ Branch 34 not taken.
✗ Branch 35 not taken.
✗ Branch 37 not taken.
✗ Branch 38 not taken.
✗ Branch 40 not taken.
✗ Branch 41 not taken.
✗ Branch 43 not taken.
✗ Branch 44 not taken.
 2 M.template bottomLeftCorner<3, 2>() << (Scalar)0, -mc(2), mc(2), (Scalar)0, -mc(1), mc(0);
428
4/8
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 2 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 2 times.
✗ Branch 12 not taken.
 2 M.template rightCols<1>().template tail<3>() = inertia.data().template tail<3>();
429
4/12
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 2 times.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 16 not taken.
✗ Branch 17 not taken.
 2 M.template rightCols<1>()[3] -= mc(0) * com(2);
430
4/12
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 2 times.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 16 not taken.
✗ Branch 17 not taken.
 2 M.template rightCols<1>()[4] -= mc(1) * com(2);
431
6/22
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 2 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 2 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 2 times.
✗ Branch 17 not taken.
✗ Branch 19 not taken.
✗ Branch 20 not taken.
✗ Branch 22 not taken.
✗ Branch 23 not taken.
✗ Branch 25 not taken.
✗ Branch 26 not taken.
✗ Branch 28 not taken.
✗ Branch 29 not taken.
✗ Branch 31 not taken.
✗ Branch 32 not taken.
 2 M.template rightCols<1>()[5] += mass * (com(0) * com(0) + com(1) * com(1));
432
433 4 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 2 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 2 Matrix63 IS;
447
2/4
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
 2 IS.template leftCols<2>() = Y.template leftCols<2>();
448
2/4
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
 2 IS.template rightCols<1>() = Y.template rightCols<1>();
449
450 2 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 877 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 3178 JointDataPlanarTpl()
535
5/9
✓ Branch 1 taken 3171 times.
✓ Branch 2 taken 5 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 5 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 5 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 5 times.
✗ Branch 12 not taken.
 3178 : joint_q(Scalar(0), Scalar(0), Scalar(1), Scalar(0))
536
3/5
✓ Branch 1 taken 5 times.
✓ Branch 2 taken 3171 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
 3178 , joint_v(TangentVector_t::Zero())
537
1/2
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
 3178 , M(Transformation_t::Identity())
538
3/5
✓ Branch 1 taken 5 times.
✓ Branch 2 taken 3171 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
 3178 , v(Motion_t::Vector3::Zero())
539
3/5
✓ Branch 1 taken 5 times.
✓ Branch 2 taken 3171 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
 3178 , U(U_t::Zero())
540
3/5
✓ Branch 1 taken 5 times.
✓ Branch 2 taken 3171 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
 3178 , Dinv(D_t::Zero())
541
3/5
✓ Branch 1 taken 5 times.
✓ Branch 2 taken 3171 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
 3178 , UDinv(UD_t::Zero())
542
3/5
✓ Branch 3 taken 5 times.
✓ Branch 4 taken 3171 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 5 times.
✗ Branch 7 not taken.
 9534 , StU(D_t::Zero())
543 {
544 3178 }
545
546 144 static std::string classname()
547 {
548
1/2
✓ Branch 2 taken 144 times.
✗ Branch 3 not taken.
 144 return std::string("JointDataPlanar");
549 }
550 3 std::string shortname() const
551 {
552 3 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 3102 JointDataDerived createData() const
572 {
573 3102 return JointDataDerived();
574 }
575
576 1 const std::vector<bool> hasConfigurationLimit() const
577 {
578
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 return {true, true, false, false};
579 }
580
581 1 const std::vector<bool> hasConfigurationLimitInTangent() const
582 {
583
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 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 6170 void calc(JointDataDerived & data, const typename Eigen::MatrixBase<ConfigVector> & qs) const
598 {
599
1/2
✓ Branch 3 taken 6158 times.
✗ Branch 4 not taken.
 6170 data.joint_q = qs.template segment<NQ>(idx_q());
600
601 6170 const Scalar &c_theta = data.joint_q(2), &s_theta = data.joint_q(3);
602
603
5/10
✓ Branch 3 taken 6158 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 6158 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 6158 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 6158 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 2 times.
✗ Branch 16 not taken.
 6170 data.M.rotation().template topLeftCorner<2, 2>() << c_theta, -s_theta, s_theta, c_theta;
604
3/6
✓ Branch 2 taken 6158 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 6158 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 6158 times.
✗ Branch 9 not taken.
 6170 data.M.translation().template head<2>() = data.joint_q.template head<2>();
605 6170 }
606
607 template<typename TangentVector>
608 void
609 1 calc(JointDataDerived & data, const Blank, const typename Eigen::MatrixBase<TangentVector> & vs)
610 const
611 {
612
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 1 data.joint_v = vs.template segment<NV>(idx_v());
613
614 #define q_dot data.joint_v
615 1 data.v.vx() = q_dot(0);
616 1 data.v.vy() = q_dot(1);
617 1 data.v.wz() = q_dot(2);
618 #undef q_dot
619 1 }
620
621 template<typename ConfigVector, typename TangentVector>
622 1043 void calc(
623 JointDataDerived & data,
624 const typename Eigen::MatrixBase<ConfigVector> & qs,
625 const typename Eigen::MatrixBase<TangentVector> & vs) const
626 {
627 1043 calc(data, qs.derived());
628
629
1/2
✓ Branch 3 taken 1037 times.
✗ Branch 4 not taken.
 1043 data.joint_v = vs.template segment<NV>(idx_v());
630
631 #define q_dot data.joint_v
632 1043 data.v.vx() = q_dot(0);
633 1043 data.v.vy() = q_dot(1);
634 1043 data.v.wz() = q_dot(2);
635 #undef q_dot
636 1043 }
637
638 template<typename VectorLike, typename Matrix6Like>
639 8 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
2/4
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
 8 data.U.template leftCols<2>() = I.template leftCols<2>();
646
2/4
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
 8 data.U.template rightCols<1>() = I.template rightCols<1>();
647
648
3/6
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 7 times.
✗ Branch 9 not taken.
 8 data.StU.template leftCols<2>() = data.U.template topRows<2>().transpose();
649
2/4
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
 8 data.StU.template rightCols<1>() = data.U.template bottomRows<1>();
650
651
1/2
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
 8 data.StU.diagonal() += armature;
652 8 internal::PerformStYSInversion<Scalar>::run(data.StU, data.Dinv);
653
654
2/4
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
 8 data.UDinv.noalias() = data.U * data.Dinv;
655
656
2/2
✓ Branch 0 taken 2 times.
✓ Branch 1 taken 5 times.
 8 if (update_I)
657
3/6
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 2 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 2 times.
✗ Branch 10 not taken.
 3 PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like, I).noalias() -= data.UDinv * data.U.transpose();
658 8 }
659
660 19551 static std::string classname()
661 {
662
1/2
✓ Branch 2 taken 19551 times.
✗ Branch 3 not taken.
 19551 return std::string("JointModelPlanar");
663 }
664 19409 std::string shortname() const
665 {
666 19409 return classname();
667 }
668
669 /// \returns An expression of *this with the Scalar type casted to NewScalar.
670 template<typename NewScalar>
671 5 JointModelPlanarTpl<NewScalar, Options> cast() const
672 {
673 typedef JointModelPlanarTpl<NewScalar, Options> ReturnType;
674 5 ReturnType res;
675 5 res.setIndexes(id(), idx_q(), idx_v());
676 5 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