GCC Code Coverage Report


Directory: ./
File: include/pinocchio/multibody/joint/joint-free-flyer.hpp
Date: 2024-08-27 18:20:05
Exec Total Coverage
Lines: 58 80 72.5%
Branches: 22 58 37.9%

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_free_flyer_hpp__
7 #define __pinocchio_multibody_joint_free_flyer_hpp__
8
9 #include "pinocchio/macros.hpp"
10 #include "pinocchio/spatial/inertia.hpp"
11 #include "pinocchio/spatial/explog.hpp"
12 #include "pinocchio/multibody/joint/joint-base.hpp"
13 #include "pinocchio/multibody/joint-motion-subspace.hpp"
14 #include "pinocchio/math/fwd.hpp"
15 #include "pinocchio/math/quaternion.hpp"
16
17 namespace pinocchio
18 {
19
20 template<typename Scalar, int Options>
21 struct JointMotionSubspaceIdentityTpl;
22
23 template<typename _Scalar, int _Options>
24 struct traits<JointMotionSubspaceIdentityTpl<_Scalar, _Options>>
25 {
26 typedef _Scalar Scalar;
27 enum
28 {
29 Options = _Options
30 };
31 typedef Eigen::Matrix<Scalar, 6, 6, Options> Matrix6;
32 enum
33 {
34 LINEAR = 0,
35 ANGULAR = 3
36 };
37 typedef MotionTpl<Scalar, Options> JointMotion;
38 typedef Eigen::Matrix<Scalar, 6, 1, Options> JointForce;
39 typedef Eigen::Matrix<Scalar, 6, 6, Options> DenseBase;
40 typedef Eigen::Matrix<Scalar, 6, 6, Options> ReducedSquaredMatrix;
41
42 typedef typename Matrix6::IdentityReturnType ConstMatrixReturnType;
43 typedef typename Matrix6::IdentityReturnType MatrixReturnType;
44 typedef typename Matrix6::IdentityReturnType StDiagonalMatrixSOperationReturnType;
45 }; // traits ConstraintRevolute
46
47 template<typename _Scalar, int _Options>
48 struct JointMotionSubspaceIdentityTpl
49 : JointMotionSubspaceBase<JointMotionSubspaceIdentityTpl<_Scalar, _Options>>
50 {
51 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
52 PINOCCHIO_CONSTRAINT_TYPEDEF_TPL(JointMotionSubspaceIdentityTpl)
53
54 enum
55 {
56 NV = 6
57 };
58
59 template<typename Vector6Like>
60 JointMotion __mult__(const Eigen::MatrixBase<Vector6Like> & vj) const
61 {
62 EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector6Like, 6);
63 return JointMotion(vj);
64 }
65
66 template<typename S1, int O1>
67 2501 typename SE3Tpl<S1, O1>::ActionMatrixType se3Action(const SE3Tpl<S1, O1> & m) const
68 {
69 2501 return m.toActionMatrix();
70 }
71
72 template<typename S1, int O1>
73 6 typename SE3Tpl<S1, O1>::ActionMatrixType se3ActionInverse(const SE3Tpl<S1, O1> & m) const
74 {
75 6 return m.toActionMatrixInverse();
76 }
77
78 int nv_impl() const
79 {
80 return NV;
81 }
82
83 struct TransposeConst : JointMotionSubspaceTransposeBase<JointMotionSubspaceIdentityTpl>
84 {
85 template<typename Derived>
86 typename ForceDense<Derived>::ToVectorConstReturnType
87 669 operator*(const ForceDense<Derived> & phi)
88 {
89 669 return phi.toVector();
90 }
91
92 /* [CRBA] MatrixBase operator* (Constraint::Transpose S, ForceSet::Block) */
93 template<typename MatrixDerived>
94 typename PINOCCHIO_EIGEN_REF_CONST_TYPE(MatrixDerived)
95 27 operator*(const Eigen::MatrixBase<MatrixDerived> & F)
96 {
97 27 return F.derived();
98 }
99 };
100
101 261 TransposeConst transpose() const
102 {
103 261 return TransposeConst();
104 }
105 1 MatrixReturnType matrix_impl() const
106 {
107 1 return DenseBase::Identity();
108 }
109
110 template<typename MotionDerived>
111 2 typename MotionDerived::ActionMatrixType motionAction(const MotionBase<MotionDerived> & v) const
112 {
113 2 return v.toActionMatrix();
114 }
115
116 bool isEqual(const JointMotionSubspaceIdentityTpl &) const
117 {
118 return true;
119 }
120
121 }; // struct JointMotionSubspaceIdentityTpl
122
123 template<typename Scalar, int Options, typename Vector6Like>
124 1534 MotionRef<const Vector6Like> operator*(
125 const JointMotionSubspaceIdentityTpl<Scalar, Options> &,
126 const Eigen::MatrixBase<Vector6Like> & v)
127 {
128 EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector6Like, 6);
129 // typedef typename JointMotionSubspaceIdentityTpl<Scalar,Options>::Motion Motion;
130 typedef MotionRef<const Vector6Like> Motion;
131
0/2
✗ Branch 3 not taken.
✗ Branch 4 not taken.
1534 return Motion(v.derived());
132 }
133
134 /* [CRBA] ForceSet operator* (Inertia Y,Constraint S) */
135 template<typename S1, int O1, typename S2, int O2>
136 inline typename InertiaTpl<S1, O1>::Matrix6
137 operator*(const InertiaTpl<S1, O1> & Y, const JointMotionSubspaceIdentityTpl<S2, O2> &)
138 {
139 return Y.matrix();
140 }
141
142 /* [ABA] Y*S operator*/
143 template<typename Matrix6Like, typename S2, int O2>
144 inline typename PINOCCHIO_EIGEN_REF_CONST_TYPE(Matrix6Like) operator*(
145 const Eigen::MatrixBase<Matrix6Like> & Y, const JointMotionSubspaceIdentityTpl<S2, O2> &)
146 {
147 return Y.derived();
148 }
149
150 template<typename S1, int O1>
151 struct SE3GroupAction<JointMotionSubspaceIdentityTpl<S1, O1>>
152 {
153 typedef typename SE3Tpl<S1, O1>::ActionMatrixType ReturnType;
154 };
155
156 template<typename S1, int O1, typename MotionDerived>
157 struct MotionAlgebraAction<JointMotionSubspaceIdentityTpl<S1, O1>, MotionDerived>
158 {
159 typedef typename SE3Tpl<S1, O1>::ActionMatrixType ReturnType;
160 };
161
162 template<typename Scalar, int Options>
163 struct JointFreeFlyerTpl;
164
165 template<typename _Scalar, int _Options>
166 struct traits<JointFreeFlyerTpl<_Scalar, _Options>>
167 {
168 enum
169 {
170 NQ = 7,
171 NV = 6
172 };
173 typedef _Scalar Scalar;
174 enum
175 {
176 Options = _Options
177 };
178 typedef JointDataFreeFlyerTpl<Scalar, Options> JointDataDerived;
179 typedef JointModelFreeFlyerTpl<Scalar, Options> JointModelDerived;
180 typedef JointMotionSubspaceIdentityTpl<Scalar, Options> Constraint_t;
181 typedef SE3Tpl<Scalar, Options> Transformation_t;
182 typedef MotionTpl<Scalar, Options> Motion_t;
183 typedef MotionZeroTpl<Scalar, Options> Bias_t;
184
185 // [ABA]
186 typedef Eigen::Matrix<Scalar, 6, NV, Options> U_t;
187 typedef Eigen::Matrix<Scalar, NV, NV, Options> D_t;
188 typedef Eigen::Matrix<Scalar, 6, NV, Options> UD_t;
189
190 typedef Eigen::Matrix<Scalar, NQ, 1, Options> ConfigVector_t;
191 typedef Eigen::Matrix<Scalar, NV, 1, Options> TangentVector_t;
192
193 PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE
194 };
195
196 template<typename _Scalar, int _Options>
197 struct traits<JointDataFreeFlyerTpl<_Scalar, _Options>>
198 {
199 typedef JointFreeFlyerTpl<_Scalar, _Options> JointDerived;
200 typedef _Scalar Scalar;
201 };
202
203 template<typename _Scalar, int _Options>
204 struct traits<JointModelFreeFlyerTpl<_Scalar, _Options>>
205 {
206 typedef JointFreeFlyerTpl<_Scalar, _Options> JointDerived;
207 typedef _Scalar Scalar;
208 };
209
210 template<typename _Scalar, int _Options>
211 struct JointDataFreeFlyerTpl : public JointDataBase<JointDataFreeFlyerTpl<_Scalar, _Options>>
212 {
213 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
214 typedef JointFreeFlyerTpl<_Scalar, _Options> JointDerived;
215 PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(JointDerived);
216 68552 PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR
217
218 ConfigVector_t joint_q;
219 TangentVector_t joint_v;
220
221 Constraint_t S;
222 Transformation_t M;
223 Motion_t v;
224 Bias_t c;
225
226 // [ABA] specific data
227 U_t U;
228 D_t Dinv;
229 UD_t UDinv;
230 D_t StU;
231
232 640 JointDataFreeFlyerTpl()
233
1/2
✓ Branch 2 taken 640 times.
✗ Branch 3 not taken.
640 : joint_q(ConfigVector_t::Zero())
234
1/2
✓ Branch 2 taken 640 times.
✗ Branch 3 not taken.
640 , joint_v(TangentVector_t::Zero())
235 640 , M(Transformation_t::Identity())
236 640 , v(Motion_t::Zero())
237
1/2
✓ Branch 2 taken 640 times.
✗ Branch 3 not taken.
640 , U(U_t::Zero())
238
1/2
✓ Branch 2 taken 640 times.
✗ Branch 3 not taken.
640 , Dinv(D_t::Zero())
239
1/2
✓ Branch 2 taken 640 times.
✗ Branch 3 not taken.
640 , UDinv(UD_t::Identity())
240
1/2
✓ Branch 3 taken 640 times.
✗ Branch 4 not taken.
1280 , StU(D_t::Zero())
241 {
242 640 joint_q[6] = Scalar(1);
243 640 }
244
245 40 static std::string classname()
246 {
247
1/2
✓ Branch 2 taken 40 times.
✗ Branch 3 not taken.
40 return std::string("JointDataFreeFlyer");
248 }
249 std::string shortname() const
250 {
251 return classname();
252 }
253
254 }; // struct JointDataFreeFlyerTpl
255
256 PINOCCHIO_JOINT_CAST_TYPE_SPECIALIZATION(JointModelFreeFlyerTpl);
257 template<typename _Scalar, int _Options>
258 struct JointModelFreeFlyerTpl : public JointModelBase<JointModelFreeFlyerTpl<_Scalar, _Options>>
259 {
260 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
261 typedef JointFreeFlyerTpl<_Scalar, _Options> JointDerived;
262 PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived);
263
264 typedef JointModelBase<JointModelFreeFlyerTpl> Base;
265 using Base::id;
266 using Base::idx_q;
267 using Base::idx_v;
268 using Base::setIndexes;
269
270 620 JointDataDerived createData() const
271 {
272 620 return JointDataDerived();
273 }
274
275 const std::vector<bool> hasConfigurationLimit() const
276 {
277 return {true, true, true, false, false, false, false};
278 }
279
280 const std::vector<bool> hasConfigurationLimitInTangent() const
281 {
282 return {true, true, true, false, false, false};
283 }
284
285 template<typename ConfigVectorLike>
286 inline void forwardKinematics(
287 Transformation_t & M, const Eigen::MatrixBase<ConfigVectorLike> & q_joint) const
288 {
289 EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigVector_t, ConfigVectorLike);
290 typedef typename Eigen::Quaternion<
291 typename ConfigVectorLike::Scalar, PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorLike)::Options>
292 Quaternion;
293 typedef Eigen::Map<const Quaternion> ConstQuaternionMap;
294
295 ConstQuaternionMap quat(q_joint.template tail<4>().data());
296 // assert(math::fabs(quat.coeffs().squaredNorm()-1.) <= sqrt(Eigen::NumTraits<typename
297 // V::Scalar>::epsilon())); TODO: check validity of the rhs precision
298 assert(math::fabs(static_cast<Scalar>(quat.coeffs().squaredNorm() - 1)) <= 1e-4);
299
300 M.rotation(quat.matrix());
301 M.translation(q_joint.template head<3>());
302 }
303
304 template<typename Vector3Derived, typename QuaternionDerived>
305 21240 EIGEN_DONT_INLINE void calc(
306 JointDataDerived & data,
307 const typename Eigen::MatrixBase<Vector3Derived> & trans,
308 const typename Eigen::QuaternionBase<QuaternionDerived> & quat) const
309 {
310
1/2
✓ Branch 2 taken 21240 times.
✗ Branch 3 not taken.
21240 data.M.translation(trans);
311
1/2
✓ Branch 2 taken 21240 times.
✗ Branch 3 not taken.
21240 data.M.rotation(quat.matrix());
312 21240 }
313
314 template<typename ConfigVector>
315 EIGEN_DONT_INLINE void
316 21459 calc(JointDataDerived & data, const typename Eigen::MatrixBase<ConfigVector> & qs) const
317 {
318 typedef typename Eigen::Quaternion<Scalar, Options> Quaternion;
319 typedef Eigen::Map<const Quaternion> ConstQuaternionMap;
320
321
3/6
✓ Branch 1 taken 21443 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 21443 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 21443 times.
✗ Branch 8 not taken.
21459 data.joint_q = qs.template segment<NQ>(idx_q());
322
2/4
✓ Branch 1 taken 21443 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 21443 times.
✗ Branch 6 not taken.
21459 ConstQuaternionMap quat(data.joint_q.template tail<4>().data());
323
324
2/4
✓ Branch 1 taken 21443 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 21443 times.
✗ Branch 5 not taken.
21459 calc(data, data.joint_q.template head<3>(), quat);
325 21459 }
326
327 template<typename TangentVector>
328 EIGEN_DONT_INLINE void
329 calc(JointDataDerived & data, const Blank, const typename Eigen::MatrixBase<TangentVector> & vs)
330 const
331 {
332 data.joint_v = vs.template segment<NV>(idx_v());
333 data.v = data.joint_v;
334 }
335
336 template<typename ConfigVector, typename TangentVector>
337 4019 EIGEN_DONT_INLINE void calc(
338 JointDataDerived & data,
339 const typename Eigen::MatrixBase<ConfigVector> & qs,
340 const typename Eigen::MatrixBase<TangentVector> & vs) const
341 {
342 4019 calc(data, qs.derived());
343
344
1/2
✓ Branch 3 taken 4019 times.
✗ Branch 4 not taken.
4019 data.joint_v = vs.template segment<NV>(idx_v());
345 4019 data.v = data.joint_v;
346 4019 }
347
348 template<typename VectorLike, typename Matrix6Like>
349 1 void calc_aba(
350 JointDataDerived & data,
351 const Eigen::MatrixBase<VectorLike> & armature,
352 const Eigen::MatrixBase<Matrix6Like> & I,
353 const bool update_I) const
354 {
355 1 data.U = I;
356 1 data.StU = I;
357
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
1 data.StU.diagonal() += armature;
358
359 1 internal::PerformStYSInversion<Scalar>::run(data.StU, data.Dinv);
360
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
1 data.UDinv.noalias() = I * data.Dinv;
361
362
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 1 times.
1 if (update_I)
363 PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like, I).noalias() -= data.UDinv * data.U.transpose();
364 1 }
365
366 41 static std::string classname()
367 {
368
1/2
✓ Branch 2 taken 41 times.
✗ Branch 3 not taken.
41 return std::string("JointModelFreeFlyer");
369 }
370 1 std::string shortname() const
371 {
372 1 return classname();
373 }
374
375 /// \returns An expression of *this with the Scalar type casted to NewScalar.
376 template<typename NewScalar>
377 JointModelFreeFlyerTpl<NewScalar, Options> cast() const
378 {
379 typedef JointModelFreeFlyerTpl<NewScalar, Options> ReturnType;
380 ReturnType res;
381 res.setIndexes(id(), idx_q(), idx_v());
382 return res;
383 }
384
385 }; // struct JointModelFreeFlyerTpl
386
387 } // namespace pinocchio
388
389 #include <boost/type_traits.hpp>
390
391 namespace boost
392 {
393 template<typename Scalar, int Options>
394 struct has_nothrow_constructor<::pinocchio::JointModelFreeFlyerTpl<Scalar, Options>>
395 : public integral_constant<bool, true>
396 {
397 };
398
399 template<typename Scalar, int Options>
400 struct has_nothrow_copy<::pinocchio::JointModelFreeFlyerTpl<Scalar, Options>>
401 : public integral_constant<bool, true>
402 {
403 };
404
405 template<typename Scalar, int Options>
406 struct has_nothrow_constructor<::pinocchio::JointDataFreeFlyerTpl<Scalar, Options>>
407 : public integral_constant<bool, true>
408 {
409 };
410
411 template<typename Scalar, int Options>
412 struct has_nothrow_copy<::pinocchio::JointDataFreeFlyerTpl<Scalar, Options>>
413 : public integral_constant<bool, true>
414 {
415 };
416 } // namespace boost
417
418 #endif // ifndef __pinocchio_multibody_joint_free_flyer_hpp__
419