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 |