Directory: | ./ |
---|---|
File: | include/pinocchio/multibody/joint/joint-mimic.hpp |
Date: | 2024-08-27 18:20:05 |
Exec | Total | Coverage | |
---|---|---|---|
Lines: | 48 | 213 | 22.5% |
Branches: | 13 | 134 | 9.7% |
Line | Branch | Exec | Source |
---|---|---|---|
1 | // | ||
2 | // Copyright (c) 2019-2021 INRIA | ||
3 | // | ||
4 | |||
5 | #ifndef __pinocchio_multibody_joint_mimic_hpp__ | ||
6 | #define __pinocchio_multibody_joint_mimic_hpp__ | ||
7 | |||
8 | #include "pinocchio/macros.hpp" | ||
9 | #include "pinocchio/multibody/joint/joint-base.hpp" | ||
10 | |||
11 | namespace pinocchio | ||
12 | { | ||
13 | |||
14 | template<class Constraint> | ||
15 | struct ScaledJointMotionSubspace; | ||
16 | |||
17 | template<class Constraint> | ||
18 | struct traits<ScaledJointMotionSubspace<Constraint>> | ||
19 | { | ||
20 | typedef typename traits<Constraint>::Scalar Scalar; | ||
21 | enum | ||
22 | { | ||
23 | Options = traits<Constraint>::Options | ||
24 | }; | ||
25 | enum | ||
26 | { | ||
27 | LINEAR = traits<Constraint>::LINEAR, | ||
28 | ANGULAR = traits<Constraint>::ANGULAR | ||
29 | }; | ||
30 | |||
31 | typedef typename traits<Constraint>::JointMotion JointMotion; | ||
32 | typedef typename traits<Constraint>::JointForce JointForce; | ||
33 | typedef typename traits<Constraint>::DenseBase DenseBase; | ||
34 | typedef typename traits<Constraint>::ReducedSquaredMatrix ReducedSquaredMatrix; | ||
35 | |||
36 | typedef typename traits<Constraint>::MatrixReturnType MatrixReturnType; | ||
37 | typedef typename traits<Constraint>::ConstMatrixReturnType ConstMatrixReturnType; | ||
38 | typedef ReducedSquaredMatrix StDiagonalMatrixSOperationReturnType; | ||
39 | }; // traits ScaledJointMotionSubspace | ||
40 | |||
41 | template<class Constraint> | ||
42 | struct SE3GroupAction<ScaledJointMotionSubspace<Constraint>> | ||
43 | { | ||
44 | typedef typename SE3GroupAction<Constraint>::ReturnType ReturnType; | ||
45 | }; | ||
46 | |||
47 | template<class Constraint, typename MotionDerived> | ||
48 | struct MotionAlgebraAction<ScaledJointMotionSubspace<Constraint>, MotionDerived> | ||
49 | { | ||
50 | typedef typename MotionAlgebraAction<Constraint, MotionDerived>::ReturnType ReturnType; | ||
51 | }; | ||
52 | |||
53 | template<class Constraint, typename ForceDerived> | ||
54 | struct ConstraintForceOp<ScaledJointMotionSubspace<Constraint>, ForceDerived> | ||
55 | { | ||
56 | typedef typename Constraint::Scalar Scalar; | ||
57 | typedef typename ConstraintForceOp<Constraint, ForceDerived>::ReturnType OriginalReturnType; | ||
58 | |||
59 | typedef typename ScalarMatrixProduct<Scalar, OriginalReturnType>::type IdealReturnType; | ||
60 | typedef Eigen::Matrix< | ||
61 | Scalar, | ||
62 | IdealReturnType::RowsAtCompileTime, | ||
63 | IdealReturnType::ColsAtCompileTime, | ||
64 | Constraint::Options> | ||
65 | ReturnType; | ||
66 | }; | ||
67 | |||
68 | template<class Constraint, typename ForceSet> | ||
69 | struct ConstraintForceSetOp<ScaledJointMotionSubspace<Constraint>, ForceSet> | ||
70 | { | ||
71 | typedef typename Constraint::Scalar Scalar; | ||
72 | typedef typename ConstraintForceSetOp<Constraint, ForceSet>::ReturnType OriginalReturnType; | ||
73 | typedef typename ScalarMatrixProduct<Scalar, OriginalReturnType>::type IdealReturnType; | ||
74 | typedef Eigen::Matrix< | ||
75 | Scalar, | ||
76 | Constraint::NV, | ||
77 | ForceSet::ColsAtCompileTime, | ||
78 | Constraint::Options | Eigen::RowMajor> | ||
79 | ReturnType; | ||
80 | }; | ||
81 | |||
82 | template<class Constraint> | ||
83 | struct ScaledJointMotionSubspace : JointMotionSubspaceBase<ScaledJointMotionSubspace<Constraint>> | ||
84 | { | ||
85 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW | ||
86 | |||
87 | PINOCCHIO_CONSTRAINT_TYPEDEF_TPL(ScaledJointMotionSubspace) | ||
88 | enum | ||
89 | { | ||
90 | NV = Constraint::NV | ||
91 | }; | ||
92 | typedef JointMotionSubspaceBase<ScaledJointMotionSubspace> Base; | ||
93 | using Base::nv; | ||
94 | |||
95 | typedef typename SE3GroupAction<Constraint>::ReturnType SE3ActionReturnType; | ||
96 | |||
97 | 168 | ScaledJointMotionSubspace() | |
98 | 168 | { | |
99 | 168 | } | |
100 | |||
101 | 120 | explicit ScaledJointMotionSubspace(const Scalar & scaling_factor) | |
102 | 120 | : m_scaling_factor(scaling_factor) | |
103 | { | ||
104 | 120 | } | |
105 | |||
106 | 180 | ScaledJointMotionSubspace(const Constraint & constraint, const Scalar & scaling_factor) | |
107 | ✗ | : m_constraint(constraint) | |
108 | 180 | , m_scaling_factor(scaling_factor) | |
109 | { | ||
110 | 180 | } | |
111 | |||
112 | ✗ | ScaledJointMotionSubspace(const ScaledJointMotionSubspace & other) | |
113 | : m_constraint(other.m_constraint) | ||
114 | ✗ | , m_scaling_factor(other.m_scaling_factor) | |
115 | { | ||
116 | } | ||
117 | |||
118 | 168 | ScaledJointMotionSubspace & operator=(const ScaledJointMotionSubspace & other) | |
119 | { | ||
120 | m_constraint = other.m_constraint; | ||
121 | 168 | m_scaling_factor = other.m_scaling_factor; | |
122 | 168 | return *this; | |
123 | } | ||
124 | |||
125 | template<typename VectorLike> | ||
126 | ✗ | JointMotion __mult__(const Eigen::MatrixBase<VectorLike> & v) const | |
127 | { | ||
128 | ✗ | assert(v.size() == nv()); | |
129 | ✗ | JointMotion jm = m_constraint * v; | |
130 | ✗ | return jm * m_scaling_factor; | |
131 | } | ||
132 | |||
133 | template<typename S1, int O1> | ||
134 | ✗ | SE3ActionReturnType se3Action(const SE3Tpl<S1, O1> & m) const | |
135 | { | ||
136 | ✗ | SE3ActionReturnType res = m_constraint.se3Action(m); | |
137 | ✗ | return m_scaling_factor * res; | |
138 | } | ||
139 | |||
140 | template<typename S1, int O1> | ||
141 | ✗ | SE3ActionReturnType se3ActionInverse(const SE3Tpl<S1, O1> & m) const | |
142 | { | ||
143 | ✗ | SE3ActionReturnType res = m_constraint.se3ActionInverse(m); | |
144 | ✗ | return m_scaling_factor * res; | |
145 | } | ||
146 | |||
147 | ✗ | int nv_impl() const | |
148 | { | ||
149 | ✗ | return m_constraint.nv(); | |
150 | } | ||
151 | |||
152 | struct TransposeConst : JointMotionSubspaceTransposeBase<ScaledJointMotionSubspace> | ||
153 | { | ||
154 | const ScaledJointMotionSubspace & ref; | ||
155 | ✗ | TransposeConst(const ScaledJointMotionSubspace & ref) | |
156 | ✗ | : ref(ref) | |
157 | { | ||
158 | } | ||
159 | |||
160 | template<typename Derived> | ||
161 | typename ConstraintForceOp<ScaledJointMotionSubspace, Derived>::ReturnType | ||
162 | ✗ | operator*(const ForceDense<Derived> & f) const | |
163 | { | ||
164 | // TODO: I don't know why, but we should a dense a return type, otherwise it fails at the | ||
165 | // evaluation level; | ||
166 | typedef | ||
167 | typename ConstraintForceOp<ScaledJointMotionSubspace, Derived>::ReturnType ReturnType; | ||
168 | ✗ | return ReturnType(ref.m_scaling_factor * (ref.m_constraint.transpose() * f)); | |
169 | } | ||
170 | |||
171 | /// [CRBA] MatrixBase operator* (Constraint::Transpose S, ForceSet::Block) | ||
172 | template<typename Derived> | ||
173 | typename ConstraintForceSetOp<ScaledJointMotionSubspace, Derived>::ReturnType | ||
174 | ✗ | operator*(const Eigen::MatrixBase<Derived> & F) const | |
175 | { | ||
176 | typedef | ||
177 | typename ConstraintForceSetOp<ScaledJointMotionSubspace, Derived>::ReturnType ReturnType; | ||
178 | ✗ | return ReturnType(ref.m_scaling_factor * (ref.m_constraint.transpose() * F)); | |
179 | } | ||
180 | |||
181 | }; // struct TransposeConst | ||
182 | |||
183 | ✗ | TransposeConst transpose() const | |
184 | { | ||
185 | ✗ | return TransposeConst(*this); | |
186 | } | ||
187 | |||
188 | ✗ | DenseBase matrix_impl() const | |
189 | { | ||
190 | ✗ | DenseBase S = m_scaling_factor * m_constraint.matrix(); | |
191 | ✗ | return S; | |
192 | } | ||
193 | |||
194 | template<typename MotionDerived> | ||
195 | typename MotionAlgebraAction<ScaledJointMotionSubspace, MotionDerived>::ReturnType | ||
196 | ✗ | motionAction(const MotionDense<MotionDerived> & m) const | |
197 | { | ||
198 | typedef typename MotionAlgebraAction<ScaledJointMotionSubspace, MotionDerived>::ReturnType | ||
199 | ReturnType; | ||
200 | ✗ | ReturnType res = m_scaling_factor * m_constraint.motionAction(m); | |
201 | ✗ | return res; | |
202 | } | ||
203 | |||
204 | ✗ | inline const Scalar & scaling() const | |
205 | { | ||
206 | ✗ | return m_scaling_factor; | |
207 | } | ||
208 | ✗ | inline Scalar & scaling() | |
209 | { | ||
210 | ✗ | return m_scaling_factor; | |
211 | } | ||
212 | |||
213 | ✗ | inline const Constraint & constraint() const | |
214 | { | ||
215 | ✗ | return m_constraint; | |
216 | } | ||
217 | ✗ | inline Constraint & constraint() | |
218 | { | ||
219 | ✗ | return m_constraint; | |
220 | } | ||
221 | |||
222 | ✗ | bool isEqual(const ScaledJointMotionSubspace & other) const | |
223 | { | ||
224 | ✗ | return internal::comparison_eq(m_constraint, other.m_constraint) | |
225 | ✗ | && internal::comparison_eq(m_scaling_factor, other.m_scaling_factor); | |
226 | } | ||
227 | |||
228 | protected: | ||
229 | Constraint m_constraint; | ||
230 | Scalar m_scaling_factor; | ||
231 | }; // struct ScaledJointMotionSubspace | ||
232 | |||
233 | namespace details | ||
234 | { | ||
235 | template<typename ParentConstraint> | ||
236 | struct StDiagonalMatrixSOperation<ScaledJointMotionSubspace<ParentConstraint>> | ||
237 | { | ||
238 | typedef ScaledJointMotionSubspace<ParentConstraint> Constraint; | ||
239 | typedef typename traits<Constraint>::StDiagonalMatrixSOperationReturnType ReturnType; | ||
240 | |||
241 | ✗ | static ReturnType run(const JointMotionSubspaceBase<Constraint> & constraint) | |
242 | { | ||
243 | ✗ | const Constraint & constraint_ = constraint.derived(); | |
244 | ✗ | return (constraint_.constraint().transpose() * constraint_.constraint()) | |
245 | ✗ | * (constraint_.scaling() * constraint_.scaling()); | |
246 | } | ||
247 | }; | ||
248 | } // namespace details | ||
249 | |||
250 | template<typename S1, int O1, typename _Constraint> | ||
251 | struct MultiplicationOp<InertiaTpl<S1, O1>, ScaledJointMotionSubspace<_Constraint>> | ||
252 | { | ||
253 | typedef InertiaTpl<S1, O1> Inertia; | ||
254 | typedef ScaledJointMotionSubspace<_Constraint> Constraint; | ||
255 | typedef typename Constraint::Scalar Scalar; | ||
256 | |||
257 | typedef typename MultiplicationOp<Inertia, _Constraint>::ReturnType OriginalReturnType; | ||
258 | // typedef typename ScalarMatrixProduct<Scalar,OriginalReturnType>::type ReturnType; | ||
259 | typedef OriginalReturnType ReturnType; | ||
260 | }; | ||
261 | |||
262 | /* [CRBA] ForceSet operator* (Inertia Y,Constraint S) */ | ||
263 | namespace impl | ||
264 | { | ||
265 | template<typename S1, int O1, typename _Constraint> | ||
266 | struct LhsMultiplicationOp<InertiaTpl<S1, O1>, ScaledJointMotionSubspace<_Constraint>> | ||
267 | { | ||
268 | typedef InertiaTpl<S1, O1> Inertia; | ||
269 | typedef ScaledJointMotionSubspace<_Constraint> Constraint; | ||
270 | typedef typename MultiplicationOp<Inertia, Constraint>::ReturnType ReturnType; | ||
271 | |||
272 | ✗ | static inline ReturnType run(const Inertia & Y, const Constraint & scaled_constraint) | |
273 | { | ||
274 | ✗ | return scaled_constraint.scaling() * (Y * scaled_constraint.constraint()); | |
275 | } | ||
276 | }; | ||
277 | } // namespace impl | ||
278 | |||
279 | template<typename M6Like, typename _Constraint> | ||
280 | struct MultiplicationOp<Eigen::MatrixBase<M6Like>, ScaledJointMotionSubspace<_Constraint>> | ||
281 | { | ||
282 | typedef typename MultiplicationOp<Inertia, _Constraint>::ReturnType OriginalReturnType; | ||
283 | typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(OriginalReturnType) ReturnType; | ||
284 | }; | ||
285 | |||
286 | /* [ABA] operator* (Inertia Y,Constraint S) */ | ||
287 | namespace impl | ||
288 | { | ||
289 | template<typename M6Like, typename _Constraint> | ||
290 | struct LhsMultiplicationOp<Eigen::MatrixBase<M6Like>, ScaledJointMotionSubspace<_Constraint>> | ||
291 | { | ||
292 | typedef ScaledJointMotionSubspace<_Constraint> Constraint; | ||
293 | typedef | ||
294 | typename MultiplicationOp<Eigen::MatrixBase<M6Like>, Constraint>::ReturnType ReturnType; | ||
295 | |||
296 | static inline ReturnType | ||
297 | ✗ | run(const Eigen::MatrixBase<M6Like> & Y, const Constraint & scaled_constraint) | |
298 | { | ||
299 | ✗ | return scaled_constraint.scaling() * (Y.derived() * scaled_constraint.constraint()); | |
300 | } | ||
301 | }; | ||
302 | } // namespace impl | ||
303 | |||
304 | template<class Joint> | ||
305 | struct JointMimic; | ||
306 | template<class JointModel> | ||
307 | struct JointModelMimic; | ||
308 | template<class JointData> | ||
309 | struct JointDataMimic; | ||
310 | |||
311 | template<class Joint> | ||
312 | struct traits<JointMimic<Joint>> | ||
313 | { | ||
314 | enum | ||
315 | { | ||
316 | NQ = traits<Joint>::NV, | ||
317 | NV = traits<Joint>::NQ | ||
318 | }; | ||
319 | typedef typename traits<Joint>::Scalar Scalar; | ||
320 | enum | ||
321 | { | ||
322 | Options = traits<Joint>::Options | ||
323 | }; | ||
324 | |||
325 | typedef typename traits<Joint>::JointDataDerived JointDataBase; | ||
326 | typedef typename traits<Joint>::JointModelDerived JointModelBase; | ||
327 | |||
328 | typedef JointDataMimic<JointDataBase> JointDataDerived; | ||
329 | typedef JointModelMimic<JointModelBase> JointModelDerived; | ||
330 | |||
331 | typedef ScaledJointMotionSubspace<typename traits<Joint>::Constraint_t> Constraint_t; | ||
332 | typedef typename traits<Joint>::Transformation_t Transformation_t; | ||
333 | typedef typename traits<Joint>::Motion_t Motion_t; | ||
334 | typedef typename traits<Joint>::Bias_t Bias_t; | ||
335 | |||
336 | // [ABA] | ||
337 | typedef typename traits<Joint>::U_t U_t; | ||
338 | typedef typename traits<Joint>::D_t D_t; | ||
339 | typedef typename traits<Joint>::UD_t UD_t; | ||
340 | |||
341 | typedef typename traits<Joint>::ConfigVector_t ConfigVector_t; | ||
342 | typedef typename traits<Joint>::TangentVector_t TangentVector_t; | ||
343 | |||
344 | PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE | ||
345 | }; | ||
346 | |||
347 | template<class Joint> | ||
348 | struct traits<JointDataMimic<Joint>> | ||
349 | { | ||
350 | typedef JointMimic<typename traits<Joint>::JointDerived> JointDerived; | ||
351 | typedef typename traits<JointDerived>::Scalar Scalar; | ||
352 | }; | ||
353 | |||
354 | template<class Joint> | ||
355 | struct traits<JointModelMimic<Joint>> | ||
356 | { | ||
357 | typedef JointMimic<typename traits<Joint>::JointDerived> JointDerived; | ||
358 | typedef typename traits<JointDerived>::Scalar Scalar; | ||
359 | }; | ||
360 | |||
361 | template<class JointData> | ||
362 | struct JointDataMimic : public JointDataBase<JointDataMimic<JointData>> | ||
363 | { | ||
364 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW | ||
365 | |||
366 | typedef typename traits<JointDataMimic>::JointDerived JointDerived; | ||
367 | typedef JointDataBase<JointDataMimic<JointData>> Base; | ||
368 | |||
369 | PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(JointDerived); | ||
370 | |||
371 | 120 | JointDataMimic() | |
372 | 120 | : m_scaling((Scalar)0) | |
373 |
1/2✓ Branch 2 taken 60 times.
✗ Branch 3 not taken.
|
120 | , joint_q(ConfigVector_t::Zero()) |
374 |
1/2✓ Branch 2 taken 60 times.
✗ Branch 3 not taken.
|
120 | , joint_v(TangentVector_t::Zero()) |
375 | 240 | , S((Scalar)0) | |
376 | { | ||
377 | 120 | } | |
378 | |||
379 | 168 | JointDataMimic(const JointDataMimic & other) | |
380 | 168 | { | |
381 | 168 | *this = other; | |
382 | 168 | } | |
383 | |||
384 | 12 | JointDataMimic(const JointDataBase<JointData> & jdata, const Scalar & scaling) | |
385 | 12 | : m_jdata_ref(jdata.derived()) | |
386 | 12 | , m_scaling(scaling) | |
387 | 24 | , S(m_jdata_ref.S, scaling) | |
388 | { | ||
389 | 12 | } | |
390 | |||
391 | 168 | JointDataMimic & operator=(const JointDataMimic & other) | |
392 | { | ||
393 | 168 | m_jdata_ref = other.m_jdata_ref; | |
394 | 168 | m_scaling = other.m_scaling; | |
395 | 168 | joint_q = other.joint_q; | |
396 | 168 | joint_v = other.joint_v; | |
397 | 168 | S = Constraint_t(m_jdata_ref.S, other.m_scaling); | |
398 | 168 | return *this; | |
399 | } | ||
400 | |||
401 | using Base::isEqual; | ||
402 | ✗ | bool isEqual(const JointDataMimic & other) const | |
403 | { | ||
404 | ✗ | return Base::isEqual(other) && internal::comparison_eq(m_jdata_ref, other.m_jdata_ref) | |
405 | ✗ | && internal::comparison_eq(m_scaling, other.m_scaling) | |
406 | ✗ | && internal::comparison_eq(joint_q, other.joint_q) | |
407 | ✗ | && internal::comparison_eq(joint_v, other.joint_v); | |
408 | } | ||
409 | |||
410 | 240 | static std::string classname() | |
411 | { | ||
412 |
5/10✓ Branch 2 taken 120 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 120 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 120 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 120 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 120 times.
✗ Branch 16 not taken.
|
240 | return std::string("JointDataMimic<") + JointData::classname() + std::string(">"); |
413 | } | ||
414 | |||
415 | ✗ | std::string shortname() const | |
416 | { | ||
417 | ✗ | return std::string("JointDataMimic<") + m_jdata_ref.shortname() + std::string(">"); | |
418 | } | ||
419 | |||
420 | // Accessors | ||
421 | ✗ | ConfigVectorTypeConstRef joint_q_accessor() const | |
422 | { | ||
423 | ✗ | return joint_q; | |
424 | } | ||
425 | ✗ | ConfigVectorTypeRef joint_q_accessor() | |
426 | { | ||
427 | ✗ | return joint_q; | |
428 | } | ||
429 | |||
430 | ✗ | TangentVectorTypeConstRef joint_v_accessor() const | |
431 | { | ||
432 | ✗ | return joint_v; | |
433 | } | ||
434 | ✗ | TangentVectorTypeRef joint_v_accessor() | |
435 | { | ||
436 | ✗ | return joint_v; | |
437 | } | ||
438 | |||
439 | ✗ | ConstraintTypeConstRef S_accessor() const | |
440 | { | ||
441 | ✗ | return S; | |
442 | } | ||
443 | ✗ | ConstraintTypeRef S_accessor() | |
444 | { | ||
445 | ✗ | return S; | |
446 | } | ||
447 | |||
448 | ✗ | TansformTypeConstRef M_accessor() const | |
449 | { | ||
450 | ✗ | return m_jdata_ref.M; | |
451 | } | ||
452 | ✗ | TansformTypeRef M_accessor() | |
453 | { | ||
454 | ✗ | return m_jdata_ref.M; | |
455 | } | ||
456 | |||
457 | ✗ | MotionTypeConstRef v_accessor() const | |
458 | { | ||
459 | ✗ | return m_jdata_ref.v; | |
460 | } | ||
461 | ✗ | MotionTypeRef v_accessor() | |
462 | { | ||
463 | ✗ | return m_jdata_ref.v; | |
464 | } | ||
465 | |||
466 | ✗ | BiasTypeConstRef c_accessor() const | |
467 | { | ||
468 | ✗ | return m_jdata_ref.c; | |
469 | } | ||
470 | ✗ | BiasTypeRef c_accessor() | |
471 | { | ||
472 | ✗ | return m_jdata_ref.c; | |
473 | } | ||
474 | |||
475 | ✗ | UTypeConstRef U_accessor() const | |
476 | { | ||
477 | ✗ | return m_jdata_ref.U; | |
478 | } | ||
479 | ✗ | UTypeRef U_accessor() | |
480 | { | ||
481 | ✗ | return m_jdata_ref.U; | |
482 | } | ||
483 | |||
484 | ✗ | DTypeConstRef Dinv_accessor() const | |
485 | { | ||
486 | ✗ | return m_jdata_ref.Dinv; | |
487 | } | ||
488 | ✗ | DTypeRef Dinv_accessor() | |
489 | { | ||
490 | ✗ | return m_jdata_ref.Dinv; | |
491 | } | ||
492 | |||
493 | ✗ | UDTypeConstRef UDinv_accessor() const | |
494 | { | ||
495 | ✗ | return m_jdata_ref.UDinv; | |
496 | } | ||
497 | ✗ | UDTypeRef UDinv_accessor() | |
498 | { | ||
499 | ✗ | return m_jdata_ref.UDinv; | |
500 | } | ||
501 | |||
502 | ✗ | DTypeConstRef StU_accessor() const | |
503 | { | ||
504 | ✗ | return m_jdata_ref.StU; | |
505 | } | ||
506 | ✗ | DTypeRef StU_accessor() | |
507 | { | ||
508 | ✗ | return m_jdata_ref.StU; | |
509 | } | ||
510 | |||
511 | template<class JointModel> | ||
512 | friend struct JointModelMimic; | ||
513 | |||
514 | const JointData & jdata() const | ||
515 | { | ||
516 | return m_jdata_ref; | ||
517 | } | ||
518 | ✗ | JointData & jdata() | |
519 | { | ||
520 | ✗ | return m_jdata_ref; | |
521 | } | ||
522 | |||
523 | const Scalar & scaling() const | ||
524 | { | ||
525 | return m_scaling; | ||
526 | } | ||
527 | ✗ | Scalar & scaling() | |
528 | { | ||
529 | ✗ | return m_scaling; | |
530 | } | ||
531 | |||
532 | ConfigVector_t & jointConfiguration() | ||
533 | { | ||
534 | return joint_q; | ||
535 | } | ||
536 | const ConfigVector_t & jointConfiguration() const | ||
537 | { | ||
538 | return joint_q; | ||
539 | } | ||
540 | |||
541 | TangentVector_t & jointVelocity() | ||
542 | { | ||
543 | return joint_v; | ||
544 | } | ||
545 | const TangentVector_t & jointVelocity() const | ||
546 | { | ||
547 | return joint_v; | ||
548 | } | ||
549 | |||
550 | protected: | ||
551 | JointData m_jdata_ref; | ||
552 | Scalar m_scaling; | ||
553 | |||
554 | /// \brief Transform configuration vector | ||
555 | ConfigVector_t joint_q; | ||
556 | /// \brief Transform velocity vector. | ||
557 | TangentVector_t joint_v; | ||
558 | |||
559 | public: | ||
560 | // data | ||
561 | Constraint_t S; | ||
562 | |||
563 | }; // struct JointDataMimic | ||
564 | |||
565 | template<typename NewScalar, typename JointModel> | ||
566 | struct CastType<NewScalar, JointModelMimic<JointModel>> | ||
567 | { | ||
568 | typedef typename CastType<NewScalar, JointModel>::type JointModelNewType; | ||
569 | typedef JointModelMimic<JointModelNewType> type; | ||
570 | }; | ||
571 | |||
572 | template<class JointModel> | ||
573 | struct JointModelMimic : public JointModelBase<JointModelMimic<JointModel>> | ||
574 | { | ||
575 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW | ||
576 | |||
577 | typedef typename traits<JointModelMimic>::JointDerived JointDerived; | ||
578 | |||
579 | PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived); | ||
580 | |||
581 | typedef JointModelBase<JointModelMimic> Base; | ||
582 | using Base::id; | ||
583 | using Base::idx_q; | ||
584 | using Base::idx_v; | ||
585 | using Base::nq; | ||
586 | using Base::nv; | ||
587 | using Base::setIndexes; | ||
588 | |||
589 | 120 | JointModelMimic() | |
590 | 120 | { | |
591 | 120 | } | |
592 | |||
593 | ✗ | JointModelMimic( | |
594 | const JointModelBase<JointModel> & jmodel, const Scalar & scaling, const Scalar & offset) | ||
595 | ✗ | : m_jmodel_ref(jmodel.derived()) | |
596 | ✗ | , m_scaling(scaling) | |
597 | ✗ | , m_offset(offset) | |
598 | { | ||
599 | } | ||
600 | |||
601 | Base & base() | ||
602 | { | ||
603 | return *static_cast<Base *>(this); | ||
604 | } | ||
605 | const Base & base() const | ||
606 | { | ||
607 | return *static_cast<const Base *>(this); | ||
608 | } | ||
609 | |||
610 | ✗ | inline int nq_impl() const | |
611 | { | ||
612 | ✗ | return 0; | |
613 | } | ||
614 | ✗ | inline int nv_impl() const | |
615 | { | ||
616 | ✗ | return 0; | |
617 | } | ||
618 | |||
619 | 18 | inline int idx_q_impl() const | |
620 | { | ||
621 | 18 | return m_jmodel_ref.idx_q(); | |
622 | } | ||
623 | ✗ | inline int idx_v_impl() const | |
624 | { | ||
625 | 33 | return m_jmodel_ref.idx_v(); | |
626 | } | ||
627 | |||
628 | ✗ | void setIndexes_impl(JointIndex id, int /*q*/, int /*v*/) | |
629 | { | ||
630 | ✗ | Base::i_id = id; // Only the id of the joint in the model is different. | |
631 | ✗ | Base::i_q = m_jmodel_ref.idx_q(); | |
632 | ✗ | Base::i_v = m_jmodel_ref.idx_v(); | |
633 | } | ||
634 | |||
635 | 12 | JointDataDerived createData() const | |
636 | { | ||
637 |
1/2✓ Branch 3 taken 6 times.
✗ Branch 4 not taken.
|
12 | return JointDataDerived(m_jmodel_ref.createData(), scaling()); |
638 | } | ||
639 | |||
640 | ✗ | const std::vector<bool> hasConfigurationLimit() const | |
641 | { | ||
642 | ✗ | return m_jmodel_ref.hasConfigurationLimit(); | |
643 | } | ||
644 | |||
645 | ✗ | const std::vector<bool> hasConfigurationLimitInTangent() const | |
646 | { | ||
647 | ✗ | return m_jmodel_ref.hasConfigurationLimitInTangent(); | |
648 | } | ||
649 | |||
650 | template<typename ConfigVector> | ||
651 | EIGEN_DONT_INLINE void | ||
652 | ✗ | calc(JointDataDerived & jdata, const typename Eigen::MatrixBase<ConfigVector> & qs) const | |
653 | { | ||
654 | typedef typename ConfigVectorAffineTransform<JointDerived>::Type AffineTransform; | ||
655 | |||
656 | ✗ | AffineTransform::run(qs.head(m_jmodel_ref.nq()), m_scaling, m_offset, jdata.joint_q); | |
657 | ✗ | m_jmodel_ref.calc(jdata.m_jdata_ref, jdata.joint_q); | |
658 | } | ||
659 | |||
660 | template<typename TangentVector> | ||
661 | ✗ | EIGEN_DONT_INLINE void calc( | |
662 | JointDataDerived & jdata, | ||
663 | const Blank blank, | ||
664 | const typename Eigen::MatrixBase<TangentVector> & vs) const | ||
665 | { | ||
666 | ✗ | jdata.joint_v = m_scaling * vs.head(m_jmodel_ref.nv()); | |
667 | ✗ | m_jmodel_ref.calc(jdata.m_jdata_ref, blank, jdata.joint_v); | |
668 | } | ||
669 | |||
670 | template<typename ConfigVector, typename TangentVector> | ||
671 | ✗ | EIGEN_DONT_INLINE void calc( | |
672 | JointDataDerived & jdata, | ||
673 | const typename Eigen::MatrixBase<ConfigVector> & qs, | ||
674 | const typename Eigen::MatrixBase<TangentVector> & vs) const | ||
675 | { | ||
676 | typedef typename ConfigVectorAffineTransform<JointDerived>::Type AffineTransform; | ||
677 | |||
678 | ✗ | AffineTransform::run(qs.head(m_jmodel_ref.nq()), m_scaling, m_offset, jdata.joint_q); | |
679 | ✗ | jdata.joint_v = m_scaling * vs.head(m_jmodel_ref.nv()); | |
680 | ✗ | m_jmodel_ref.calc(jdata.m_jdata_ref, jdata.joint_q, jdata.joint_v); | |
681 | } | ||
682 | |||
683 | template<typename VectorLike, typename Matrix6Like> | ||
684 | ✗ | void calc_aba( | |
685 | JointDataDerived & data, | ||
686 | const Eigen::MatrixBase<VectorLike> & armature, | ||
687 | const Eigen::MatrixBase<Matrix6Like> & I, | ||
688 | const bool update_I) const | ||
689 | { | ||
690 | // TODO: fixme | ||
691 | ✗ | m_jmodel_ref.calc_aba( | |
692 | ✗ | data.m_jdata_ref, armature, PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like, I), update_I); | |
693 | } | ||
694 | |||
695 | 240 | static std::string classname() | |
696 | { | ||
697 |
5/10✓ Branch 2 taken 120 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 120 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 120 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 120 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 120 times.
✗ Branch 16 not taken.
|
240 | return std::string("JointModelMimic<") + JointModel::classname() + std::string(">"); |
698 | ; | ||
699 | } | ||
700 | |||
701 | ✗ | std::string shortname() const | |
702 | { | ||
703 | ✗ | return std::string("JointModelMimic<") + m_jmodel_ref.shortname() + std::string(">"); | |
704 | } | ||
705 | |||
706 | /// \returns An expression of *this with the Scalar type casted to NewScalar. | ||
707 | template<typename NewScalar> | ||
708 | ✗ | typename CastType<NewScalar, JointModelMimic>::type cast() const | |
709 | { | ||
710 | typedef typename CastType<NewScalar, JointModelMimic>::type ReturnType; | ||
711 | |||
712 | ✗ | ReturnType res( | |
713 | ✗ | m_jmodel_ref.template cast<NewScalar>(), pinocchio::cast<NewScalar>(m_scaling), | |
714 | ✗ | pinocchio::cast<NewScalar>(m_offset)); | |
715 | ✗ | res.setIndexes(id(), idx_q(), idx_v()); | |
716 | ✗ | return res; | |
717 | } | ||
718 | |||
719 | ✗ | const JointModel & jmodel() const | |
720 | { | ||
721 | ✗ | return m_jmodel_ref; | |
722 | } | ||
723 | ✗ | JointModel & jmodel() | |
724 | { | ||
725 | ✗ | return m_jmodel_ref; | |
726 | } | ||
727 | |||
728 | 12 | const Scalar & scaling() const | |
729 | { | ||
730 | 12 | return m_scaling; | |
731 | } | ||
732 | ✗ | Scalar & scaling() | |
733 | { | ||
734 | ✗ | return m_scaling; | |
735 | } | ||
736 | |||
737 | const Scalar & offset() const | ||
738 | { | ||
739 | return m_offset; | ||
740 | } | ||
741 | ✗ | Scalar & offset() | |
742 | { | ||
743 | ✗ | return m_offset; | |
744 | } | ||
745 | |||
746 | protected: | ||
747 | // data | ||
748 | JointModel m_jmodel_ref; | ||
749 | Scalar m_scaling, m_offset; | ||
750 | |||
751 | public: | ||
752 | /* Acces to dedicated segment in robot config space. */ | ||
753 | // Const access | ||
754 | template<typename D> | ||
755 | typename SizeDepType<NQ>::template SegmentReturn<D>::ConstType | ||
756 | ✗ | jointConfigSelector_impl(const Eigen::MatrixBase<D> & a) const | |
757 | { | ||
758 | ✗ | return SizeDepType<NQ>::segment(a.derived(), m_jmodel_ref.idx_q(), m_jmodel_ref.nq()); | |
759 | } | ||
760 | |||
761 | // Non-const access | ||
762 | template<typename D> | ||
763 | typename SizeDepType<NQ>::template SegmentReturn<D>::Type | ||
764 | ✗ | jointConfigSelector_impl(Eigen::MatrixBase<D> & a) const | |
765 | { | ||
766 | ✗ | return SizeDepType<NQ>::segment(a.derived(), m_jmodel_ref.idx_q(), m_jmodel_ref.nq()); | |
767 | } | ||
768 | |||
769 | /* Acces to dedicated segment in robot config velocity space. */ | ||
770 | // Const access | ||
771 | template<typename D> | ||
772 | typename SizeDepType<NV>::template SegmentReturn<D>::ConstType | ||
773 | ✗ | jointVelocitySelector_impl(const Eigen::MatrixBase<D> & a) const | |
774 | { | ||
775 | ✗ | return SizeDepType<NV>::segment(a.derived(), m_jmodel_ref.idx_v(), m_jmodel_ref.nv()); | |
776 | } | ||
777 | |||
778 | // Non-const access | ||
779 | template<typename D> | ||
780 | typename SizeDepType<NV>::template SegmentReturn<D>::Type | ||
781 | ✗ | jointVelocitySelector_impl(Eigen::MatrixBase<D> & a) const | |
782 | { | ||
783 | ✗ | return SizeDepType<NV>::segment(a.derived(), m_jmodel_ref.idx_v(), m_jmodel_ref.nv()); | |
784 | } | ||
785 | |||
786 | /* Acces to dedicated columns in a ForceSet or MotionSet matrix.*/ | ||
787 | // Const access | ||
788 | template<typename D> | ||
789 | typename SizeDepType<NV>::template ColsReturn<D>::ConstType | ||
790 | ✗ | jointCols_impl(const Eigen::MatrixBase<D> & A) const | |
791 | { | ||
792 | ✗ | return SizeDepType<NV>::middleCols(A.derived(), m_jmodel_ref.idx_v(), m_jmodel_ref.nv()); | |
793 | } | ||
794 | |||
795 | // Non-const access | ||
796 | template<typename D> | ||
797 | typename SizeDepType<NV>::template ColsReturn<D>::Type | ||
798 | ✗ | jointCols_impl(Eigen::MatrixBase<D> & A) const | |
799 | { | ||
800 | ✗ | return SizeDepType<NV>::middleCols(A.derived(), m_jmodel_ref.idx_v(), m_jmodel_ref.nv()); | |
801 | } | ||
802 | |||
803 | /* Acces to dedicated rows in a matrix.*/ | ||
804 | // Const access | ||
805 | template<typename D> | ||
806 | typename SizeDepType<NV>::template RowsReturn<D>::ConstType | ||
807 | ✗ | jointRows_impl(const Eigen::MatrixBase<D> & A) const | |
808 | { | ||
809 | ✗ | return SizeDepType<NV>::middleRows(A.derived(), m_jmodel_ref.idx_v(), m_jmodel_ref.nv()); | |
810 | } | ||
811 | |||
812 | // Non-const access | ||
813 | template<typename D> | ||
814 | typename SizeDepType<NV>::template RowsReturn<D>::Type | ||
815 | ✗ | jointRows_impl(Eigen::MatrixBase<D> & A) const | |
816 | { | ||
817 | ✗ | return SizeDepType<NV>::middleRows(A.derived(), m_jmodel_ref.idx_v(), m_jmodel_ref.nv()); | |
818 | } | ||
819 | |||
820 | /// \brief Returns a block of dimension nv()xnv() located at position idx_v(),idx_v() in the | ||
821 | /// matrix Mat | ||
822 | // Const access | ||
823 | template<typename D> | ||
824 | typename SizeDepType<NV>::template BlockReturn<D>::ConstType | ||
825 | jointBlock_impl(const Eigen::MatrixBase<D> & Mat) const | ||
826 | { | ||
827 | return SizeDepType<NV>::block( | ||
828 | Mat.derived(), m_jmodel_ref.idx_v(), m_jmodel_ref.idx_v(), m_jmodel_ref.nv(), | ||
829 | m_jmodel_ref.nv()); | ||
830 | } | ||
831 | |||
832 | // Non-const access | ||
833 | template<typename D> | ||
834 | typename SizeDepType<NV>::template BlockReturn<D>::Type | ||
835 | ✗ | jointBlock_impl(Eigen::MatrixBase<D> & Mat) const | |
836 | { | ||
837 | ✗ | return SizeDepType<NV>::block( | |
838 | ✗ | Mat.derived(), m_jmodel_ref.idx_v(), m_jmodel_ref.idx_v(), m_jmodel_ref.nv(), | |
839 | ✗ | m_jmodel_ref.nv()); | |
840 | } | ||
841 | |||
842 | }; // struct JointModelMimic | ||
843 | |||
844 | } // namespace pinocchio | ||
845 | |||
846 | #endif // ifndef __pinocchio_multibody_joint_mimic_hpp__ | ||
847 |