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