Directory: | ./ |
---|---|
File: | include/pinocchio/multibody/joint/joint-revolute.hpp |
Date: | 2024-08-27 18:20:05 |
Exec | Total | Coverage | |
---|---|---|---|
Lines: | 141 | 212 | 66.5% |
Branches: | 64 | 241 | 26.6% |
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_revolute_hpp__ | ||
7 | #define __pinocchio_multibody_joint_revolute_hpp__ | ||
8 | |||
9 | #include "pinocchio/math/sincos.hpp" | ||
10 | #include "pinocchio/spatial/inertia.hpp" | ||
11 | #include "pinocchio/multibody/joint-motion-subspace.hpp" | ||
12 | #include "pinocchio/multibody/joint/joint-base.hpp" | ||
13 | #include "pinocchio/spatial/spatial-axis.hpp" | ||
14 | #include "pinocchio/utils/axis-label.hpp" | ||
15 | |||
16 | namespace pinocchio | ||
17 | { | ||
18 | |||
19 | template<typename Scalar, int Options, int axis> | ||
20 | struct MotionRevoluteTpl; | ||
21 | |||
22 | template<typename Scalar, int Options, int axis> | ||
23 | struct SE3GroupAction<MotionRevoluteTpl<Scalar, Options, axis>> | ||
24 | { | ||
25 | typedef MotionTpl<Scalar, Options> ReturnType; | ||
26 | }; | ||
27 | |||
28 | template<typename Scalar, int Options, int axis, typename MotionDerived> | ||
29 | struct MotionAlgebraAction<MotionRevoluteTpl<Scalar, Options, axis>, MotionDerived> | ||
30 | { | ||
31 | typedef MotionTpl<Scalar, Options> ReturnType; | ||
32 | }; | ||
33 | |||
34 | template<typename _Scalar, int _Options, int axis> | ||
35 | struct traits<MotionRevoluteTpl<_Scalar, _Options, axis>> | ||
36 | { | ||
37 | typedef _Scalar Scalar; | ||
38 | enum | ||
39 | { | ||
40 | Options = _Options | ||
41 | }; | ||
42 | typedef Eigen::Matrix<Scalar, 3, 1, Options> Vector3; | ||
43 | typedef Eigen::Matrix<Scalar, 6, 1, Options> Vector6; | ||
44 | typedef Eigen::Matrix<Scalar, 4, 4, Options> Matrix4; | ||
45 | typedef Eigen::Matrix<Scalar, 6, 6, Options> Matrix6; | ||
46 | typedef typename PINOCCHIO_EIGEN_REF_CONST_TYPE(Vector6) ToVectorConstReturnType; | ||
47 | typedef typename PINOCCHIO_EIGEN_REF_TYPE(Vector6) ToVectorReturnType; | ||
48 | typedef Vector3 AngularType; | ||
49 | typedef Vector3 LinearType; | ||
50 | typedef const Vector3 ConstAngularType; | ||
51 | typedef const Vector3 ConstLinearType; | ||
52 | typedef Matrix6 ActionMatrixType; | ||
53 | typedef Matrix4 HomogeneousMatrixType; | ||
54 | typedef MotionTpl<Scalar, Options> MotionPlain; | ||
55 | typedef MotionPlain PlainReturnType; | ||
56 | enum | ||
57 | { | ||
58 | LINEAR = 0, | ||
59 | ANGULAR = 3 | ||
60 | }; | ||
61 | }; // traits MotionRevoluteTpl | ||
62 | |||
63 | template<typename Scalar, int Options, int axis> | ||
64 | struct TransformRevoluteTpl; | ||
65 | |||
66 | template<typename _Scalar, int _Options, int _axis> | ||
67 | struct traits<TransformRevoluteTpl<_Scalar, _Options, _axis>> | ||
68 | { | ||
69 | enum | ||
70 | { | ||
71 | axis = _axis, | ||
72 | Options = _Options, | ||
73 | LINEAR = 0, | ||
74 | ANGULAR = 3 | ||
75 | }; | ||
76 | typedef _Scalar Scalar; | ||
77 | typedef SE3Tpl<Scalar, Options> PlainType; | ||
78 | typedef Eigen::Matrix<Scalar, 3, 1, Options> Vector3; | ||
79 | typedef Eigen::Matrix<Scalar, 3, 3, Options> Matrix3; | ||
80 | typedef Matrix3 AngularType; | ||
81 | typedef Matrix3 AngularRef; | ||
82 | typedef Matrix3 ConstAngularRef; | ||
83 | typedef typename Vector3::ConstantReturnType LinearType; | ||
84 | typedef typename Vector3::ConstantReturnType LinearRef; | ||
85 | typedef const typename Vector3::ConstantReturnType ConstLinearRef; | ||
86 | typedef typename traits<PlainType>::ActionMatrixType ActionMatrixType; | ||
87 | typedef typename traits<PlainType>::HomogeneousMatrixType HomogeneousMatrixType; | ||
88 | }; // traits TransformRevoluteTpl | ||
89 | |||
90 | template<typename Scalar, int Options, int axis> | ||
91 | struct SE3GroupAction<TransformRevoluteTpl<Scalar, Options, axis>> | ||
92 | { | ||
93 | typedef typename traits<TransformRevoluteTpl<Scalar, Options, axis>>::PlainType ReturnType; | ||
94 | }; | ||
95 | |||
96 | template<typename _Scalar, int _Options, int axis> | ||
97 | struct TransformRevoluteTpl : SE3Base<TransformRevoluteTpl<_Scalar, _Options, axis>> | ||
98 | { | ||
99 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW | ||
100 | PINOCCHIO_SE3_TYPEDEF_TPL(TransformRevoluteTpl); | ||
101 | |||
102 | ✗ | TransformRevoluteTpl() | |
103 | { | ||
104 | } | ||
105 | 30834 | TransformRevoluteTpl(const Scalar & sin, const Scalar & cos) | |
106 | 30834 | : m_sin(sin) | |
107 | 30834 | , m_cos(cos) | |
108 | { | ||
109 | 30834 | } | |
110 | |||
111 | 1127936 | PlainType plain() const | |
112 | { | ||
113 | 1127936 | PlainType res(PlainType::Identity()); | |
114 | 1127936 | _setRotation(res.rotation()); | |
115 | 1127936 | return res; | |
116 | } | ||
117 | |||
118 | 1127936 | operator PlainType() const | |
119 | { | ||
120 | 1127936 | return plain(); | |
121 | } | ||
122 | |||
123 | template<typename S2, int O2> | ||
124 | typename SE3GroupAction<TransformRevoluteTpl>::ReturnType | ||
125 | se3action(const SE3Tpl<S2, O2> & m) const | ||
126 | { | ||
127 | typedef typename SE3GroupAction<TransformRevoluteTpl>::ReturnType ReturnType; | ||
128 | ReturnType res; | ||
129 | switch (axis) | ||
130 | { | ||
131 | case 0: { | ||
132 | res.rotation().col(0) = m.rotation().col(0); | ||
133 | res.rotation().col(1).noalias() = m_cos * m.rotation().col(1) + m_sin * m.rotation().col(2); | ||
134 | res.rotation().col(2).noalias() = res.rotation().col(0).cross(res.rotation().col(1)); | ||
135 | break; | ||
136 | } | ||
137 | case 1: { | ||
138 | res.rotation().col(2).noalias() = m_cos * m.rotation().col(2) + m_sin * m.rotation().col(0); | ||
139 | res.rotation().col(1) = m.rotation().col(1); | ||
140 | res.rotation().col(0).noalias() = res.rotation().col(1).cross(res.rotation().col(2)); | ||
141 | break; | ||
142 | } | ||
143 | case 2: { | ||
144 | res.rotation().col(0).noalias() = m_cos * m.rotation().col(0) + m_sin * m.rotation().col(1); | ||
145 | res.rotation().col(1).noalias() = res.rotation().col(2).cross(res.rotation().col(0)); | ||
146 | res.rotation().col(2) = m.rotation().col(2); | ||
147 | break; | ||
148 | } | ||
149 | default: { | ||
150 | assert(false && "must never happened"); | ||
151 | break; | ||
152 | } | ||
153 | } | ||
154 | res.translation() = m.translation(); | ||
155 | return res; | ||
156 | } | ||
157 | |||
158 | const Scalar & sin() const | ||
159 | { | ||
160 | return m_sin; | ||
161 | } | ||
162 | ✗ | Scalar & sin() | |
163 | { | ||
164 | ✗ | return m_sin; | |
165 | } | ||
166 | |||
167 | const Scalar & cos() const | ||
168 | { | ||
169 | return m_cos; | ||
170 | } | ||
171 | ✗ | Scalar & cos() | |
172 | { | ||
173 | ✗ | return m_cos; | |
174 | } | ||
175 | |||
176 | template<typename OtherScalar> | ||
177 | 1127936 | void setValues(const OtherScalar & sin, const OtherScalar & cos) | |
178 | { | ||
179 | 1127936 | m_sin = sin; | |
180 | 1127936 | m_cos = cos; | |
181 | 1127936 | } | |
182 | |||
183 | ✗ | LinearType translation() const | |
184 | { | ||
185 | ✗ | return LinearType::PlainObject::Zero(3); | |
186 | } | ||
187 | ✗ | AngularType rotation() const | |
188 | { | ||
189 | ✗ | AngularType m(AngularType::Identity()); | |
190 | ✗ | _setRotation(m); | |
191 | ✗ | return m; | |
192 | } | ||
193 | |||
194 | ✗ | bool isEqual(const TransformRevoluteTpl & other) const | |
195 | { | ||
196 | ✗ | return internal::comparison_eq(m_cos, other.m_cos) | |
197 | ✗ | && internal::comparison_eq(m_sin, other.m_sin); | |
198 | } | ||
199 | |||
200 | protected: | ||
201 | Scalar m_sin, m_cos; | ||
202 | 1127936 | inline void _setRotation(typename PlainType::AngularRef & rot) const | |
203 | { | ||
204 | switch (axis) | ||
205 | { | ||
206 | case 0: { | ||
207 | 342930 | rot.coeffRef(1, 1) = m_cos; | |
208 | 342930 | rot.coeffRef(1, 2) = -m_sin; | |
209 | 342930 | rot.coeffRef(2, 1) = m_sin; | |
210 | 342930 | rot.coeffRef(2, 2) = m_cos; | |
211 | 342930 | break; | |
212 | } | ||
213 | case 1: { | ||
214 | 535854 | rot.coeffRef(0, 0) = m_cos; | |
215 | 535854 | rot.coeffRef(0, 2) = m_sin; | |
216 | 535854 | rot.coeffRef(2, 0) = -m_sin; | |
217 | 535854 | rot.coeffRef(2, 2) = m_cos; | |
218 | 535854 | break; | |
219 | } | ||
220 | case 2: { | ||
221 | 249152 | rot.coeffRef(0, 0) = m_cos; | |
222 | 249152 | rot.coeffRef(0, 1) = -m_sin; | |
223 | 249152 | rot.coeffRef(1, 0) = m_sin; | |
224 | 249152 | rot.coeffRef(1, 1) = m_cos; | |
225 | 249152 | break; | |
226 | } | ||
227 | default: { | ||
228 | assert(false && "must never happened"); | ||
229 | break; | ||
230 | } | ||
231 | } | ||
232 | 1127936 | } | |
233 | }; | ||
234 | |||
235 | template<typename _Scalar, int _Options, int axis> | ||
236 | struct MotionRevoluteTpl : MotionBase<MotionRevoluteTpl<_Scalar, _Options, axis>> | ||
237 | { | ||
238 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW | ||
239 | |||
240 | MOTION_TYPEDEF_TPL(MotionRevoluteTpl); | ||
241 | typedef SpatialAxis<axis + ANGULAR> Axis; | ||
242 | typedef typename Axis::CartesianAxis3 CartesianAxis3; | ||
243 | |||
244 | ✗ | MotionRevoluteTpl() | |
245 | { | ||
246 | } | ||
247 | |||
248 | 86106 | MotionRevoluteTpl(const Scalar & w) | |
249 | 86106 | : m_w(w) | |
250 | { | ||
251 | 86106 | } | |
252 | |||
253 | template<typename Vector1Like> | ||
254 | MotionRevoluteTpl(const Eigen::MatrixBase<Vector1Like> & v) | ||
255 | : m_w(v[0]) | ||
256 | { | ||
257 | using namespace Eigen; | ||
258 | EIGEN_STATIC_ASSERT_SIZE_1x1(Vector1Like); | ||
259 | } | ||
260 | |||
261 | 112 | inline PlainReturnType plain() const | |
262 | { | ||
263 |
1/2✓ Branch 1 taken 56 times.
✗ Branch 2 not taken.
|
224 | return Axis() * m_w; |
264 | } | ||
265 | |||
266 | template<typename OtherScalar> | ||
267 | ✗ | MotionRevoluteTpl __mult__(const OtherScalar & alpha) const | |
268 | { | ||
269 | ✗ | return MotionRevoluteTpl(alpha * m_w); | |
270 | } | ||
271 | |||
272 | template<typename MotionDerived> | ||
273 | 97264 | void setTo(MotionDense<MotionDerived> & m) const | |
274 | { | ||
275 |
1/2✓ Branch 2 taken 48632 times.
✗ Branch 3 not taken.
|
97264 | m.linear().setZero(); |
276 |
2/2✓ Branch 0 taken 145896 times.
✓ Branch 1 taken 48632 times.
|
389056 | for (Eigen::DenseIndex k = 0; k < 3; ++k) |
277 | { | ||
278 |
3/4✓ Branch 0 taken 48632 times.
✓ Branch 1 taken 97264 times.
✓ Branch 4 taken 145896 times.
✗ Branch 5 not taken.
|
291792 | m.angular()[k] = k == axis ? m_w : Scalar(0); |
279 | } | ||
280 | 97264 | } | |
281 | |||
282 | template<typename MotionDerived> | ||
283 | 79484 | inline void addTo(MotionDense<MotionDerived> & v) const | |
284 | { | ||
285 | typedef typename MotionDense<MotionDerived>::Scalar OtherScalar; | ||
286 |
1/2✓ Branch 2 taken 39742 times.
✗ Branch 3 not taken.
|
79484 | v.angular()[axis] += (OtherScalar)m_w; |
287 | 79484 | } | |
288 | |||
289 | template<typename S2, int O2, typename D2> | ||
290 | 81912 | inline void se3Action_impl(const SE3Tpl<S2, O2> & m, MotionDense<D2> & v) const | |
291 | { | ||
292 |
4/8✓ Branch 3 taken 40956 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 40956 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 40956 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 40956 times.
✗ Branch 13 not taken.
|
81912 | v.angular().noalias() = m.rotation().col(axis) * m_w; |
293 |
4/8✓ Branch 3 taken 40956 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 40956 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 40956 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 40956 times.
✗ Branch 13 not taken.
|
81912 | v.linear().noalias() = m.translation().cross(v.angular()); |
294 | 81912 | } | |
295 | |||
296 | template<typename S2, int O2> | ||
297 | 81912 | MotionPlain se3Action_impl(const SE3Tpl<S2, O2> & m) const | |
298 | { | ||
299 | 81912 | MotionPlain res; | |
300 | 81912 | se3Action_impl(m, res); | |
301 | 81912 | return res; | |
302 | } | ||
303 | |||
304 | template<typename S2, int O2, typename D2> | ||
305 | ✗ | void se3ActionInverse_impl(const SE3Tpl<S2, O2> & m, MotionDense<D2> & v) const | |
306 | { | ||
307 | // Linear | ||
308 | ✗ | CartesianAxis3::alphaCross(m_w, m.translation(), v.angular()); | |
309 | ✗ | v.linear().noalias() = m.rotation().transpose() * v.angular(); | |
310 | |||
311 | // Angular | ||
312 | ✗ | v.angular().noalias() = m.rotation().transpose().col(axis) * m_w; | |
313 | } | ||
314 | |||
315 | template<typename S2, int O2> | ||
316 | ✗ | MotionPlain se3ActionInverse_impl(const SE3Tpl<S2, O2> & m) const | |
317 | { | ||
318 | ✗ | MotionPlain res; | |
319 | ✗ | se3ActionInverse_impl(m, res); | |
320 | ✗ | return res; | |
321 | } | ||
322 | |||
323 | template<typename M1, typename M2> | ||
324 | 91680 | EIGEN_STRONG_INLINE void motionAction(const MotionDense<M1> & v, MotionDense<M2> & mout) const | |
325 | { | ||
326 | // Linear | ||
327 |
2/4✓ Branch 2 taken 45840 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 45840 times.
✗ Branch 6 not taken.
|
91680 | CartesianAxis3::alphaCross(-m_w, v.linear(), mout.linear()); |
328 | |||
329 | // Angular | ||
330 |
2/4✓ Branch 2 taken 45840 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 45840 times.
✗ Branch 6 not taken.
|
91680 | CartesianAxis3::alphaCross(-m_w, v.angular(), mout.angular()); |
331 | 91680 | } | |
332 | |||
333 | template<typename M1> | ||
334 | 91680 | MotionPlain motionAction(const MotionDense<M1> & v) const | |
335 | { | ||
336 | 91680 | MotionPlain res; | |
337 | 91680 | motionAction(v, res); | |
338 | 91680 | return res; | |
339 | } | ||
340 | |||
341 | 179176 | Scalar & angularRate() | |
342 | { | ||
343 | 179176 | return m_w; | |
344 | } | ||
345 | const Scalar & angularRate() const | ||
346 | { | ||
347 | return m_w; | ||
348 | } | ||
349 | |||
350 | ✗ | bool isEqual_impl(const MotionRevoluteTpl & other) const | |
351 | { | ||
352 | ✗ | return internal::comparison_eq(m_w, other.m_w); | |
353 | } | ||
354 | |||
355 | protected: | ||
356 | Scalar m_w; | ||
357 | }; // struct MotionRevoluteTpl | ||
358 | |||
359 | template<typename S1, int O1, int axis, typename MotionDerived> | ||
360 | typename MotionDerived::MotionPlain | ||
361 | 60736 | operator+(const MotionRevoluteTpl<S1, O1, axis> & m1, const MotionDense<MotionDerived> & m2) | |
362 | { | ||
363 | 60736 | typename MotionDerived::MotionPlain res(m2); | |
364 | 60736 | res += m1; | |
365 | 60736 | return res; | |
366 | } | ||
367 | |||
368 | template<typename MotionDerived, typename S2, int O2, int axis> | ||
369 | EIGEN_STRONG_INLINE typename MotionDerived::MotionPlain | ||
370 | 91680 | operator^(const MotionDense<MotionDerived> & m1, const MotionRevoluteTpl<S2, O2, axis> & m2) | |
371 | { | ||
372 | 91680 | return m2.motionAction(m1); | |
373 | } | ||
374 | |||
375 | template<typename Scalar, int Options, int axis> | ||
376 | struct JointMotionSubspaceRevoluteTpl; | ||
377 | |||
378 | template<typename Scalar, int Options, int axis> | ||
379 | struct SE3GroupAction<JointMotionSubspaceRevoluteTpl<Scalar, Options, axis>> | ||
380 | { | ||
381 | typedef Eigen::Matrix<Scalar, 6, 1, Options> ReturnType; | ||
382 | }; | ||
383 | |||
384 | template<typename Scalar, int Options, int axis, typename MotionDerived> | ||
385 | struct MotionAlgebraAction<JointMotionSubspaceRevoluteTpl<Scalar, Options, axis>, MotionDerived> | ||
386 | { | ||
387 | typedef Eigen::Matrix<Scalar, 6, 1, Options> ReturnType; | ||
388 | }; | ||
389 | |||
390 | template<typename Scalar, int Options, int axis, typename ForceDerived> | ||
391 | struct ConstraintForceOp<JointMotionSubspaceRevoluteTpl<Scalar, Options, axis>, ForceDerived> | ||
392 | { | ||
393 | typedef typename ForceDense< | ||
394 | ForceDerived>::ConstAngularType::template ConstFixedSegmentReturnType<1>::Type ReturnType; | ||
395 | }; | ||
396 | |||
397 | template<typename Scalar, int Options, int axis, typename ForceSet> | ||
398 | struct ConstraintForceSetOp<JointMotionSubspaceRevoluteTpl<Scalar, Options, axis>, ForceSet> | ||
399 | { | ||
400 | typedef typename Eigen::MatrixBase<ForceSet>::ConstRowXpr ReturnType; | ||
401 | }; | ||
402 | |||
403 | template<typename _Scalar, int _Options, int axis> | ||
404 | struct traits<JointMotionSubspaceRevoluteTpl<_Scalar, _Options, axis>> | ||
405 | { | ||
406 | typedef _Scalar Scalar; | ||
407 | enum | ||
408 | { | ||
409 | Options = _Options | ||
410 | }; | ||
411 | enum | ||
412 | { | ||
413 | LINEAR = 0, | ||
414 | ANGULAR = 3 | ||
415 | }; | ||
416 | |||
417 | typedef MotionRevoluteTpl<Scalar, Options, axis> JointMotion; | ||
418 | typedef Eigen::Matrix<Scalar, 1, 1, Options> JointForce; | ||
419 | typedef Eigen::Matrix<Scalar, 6, 1, Options> DenseBase; | ||
420 | typedef Eigen::Matrix<Scalar, 1, 1, Options> ReducedSquaredMatrix; | ||
421 | |||
422 | typedef DenseBase MatrixReturnType; | ||
423 | typedef const DenseBase ConstMatrixReturnType; | ||
424 | |||
425 | typedef typename ReducedSquaredMatrix::IdentityReturnType StDiagonalMatrixSOperationReturnType; | ||
426 | }; // traits JointMotionSubspaceRevoluteTpl | ||
427 | |||
428 | template<typename _Scalar, int _Options, int axis> | ||
429 | struct JointMotionSubspaceRevoluteTpl | ||
430 | : JointMotionSubspaceBase<JointMotionSubspaceRevoluteTpl<_Scalar, _Options, axis>> | ||
431 | { | ||
432 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW | ||
433 | |||
434 | PINOCCHIO_CONSTRAINT_TYPEDEF_TPL(JointMotionSubspaceRevoluteTpl) | ||
435 | enum | ||
436 | { | ||
437 | NV = 1 | ||
438 | }; | ||
439 | |||
440 | typedef SpatialAxis<ANGULAR + axis> Axis; | ||
441 | |||
442 | 31142 | JointMotionSubspaceRevoluteTpl() | |
443 | { | ||
444 | 31142 | } | |
445 | |||
446 | template<typename Vector1Like> | ||
447 | 79798 | JointMotion __mult__(const Eigen::MatrixBase<Vector1Like> & v) const | |
448 | { | ||
449 | 79798 | return JointMotion(v[0]); | |
450 | } | ||
451 | |||
452 | template<typename S1, int O1> | ||
453 | typename SE3GroupAction<JointMotionSubspaceRevoluteTpl>::ReturnType | ||
454 | 108900 | se3Action(const SE3Tpl<S1, O1> & m) const | |
455 | { | ||
456 | typedef typename SE3GroupAction<JointMotionSubspaceRevoluteTpl>::ReturnType ReturnType; | ||
457 | 108900 | ReturnType res; | |
458 |
3/6✓ Branch 4 taken 54450 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 54450 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 54450 times.
✗ Branch 11 not taken.
|
108900 | res.template segment<3>(LINEAR) = m.translation().cross(m.rotation().col(axis)); |
459 |
2/4✓ Branch 3 taken 54450 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 54450 times.
✗ Branch 7 not taken.
|
108900 | res.template segment<3>(ANGULAR) = m.rotation().col(axis); |
460 | 108900 | return res; | |
461 | } | ||
462 | |||
463 | template<typename S1, int O1> | ||
464 | typename SE3GroupAction<JointMotionSubspaceRevoluteTpl>::ReturnType | ||
465 | 2208 | se3ActionInverse(const SE3Tpl<S1, O1> & m) const | |
466 | { | ||
467 | typedef typename SE3GroupAction<JointMotionSubspaceRevoluteTpl>::ReturnType ReturnType; | ||
468 | typedef typename Axis::CartesianAxis3 CartesianAxis3; | ||
469 | 2208 | ReturnType res; | |
470 |
3/6✓ Branch 1 taken 1104 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1104 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1104 times.
✗ Branch 8 not taken.
|
2208 | res.template segment<3>(LINEAR).noalias() = |
471 |
3/6✓ Branch 3 taken 1104 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1104 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 1104 times.
✗ Branch 10 not taken.
|
2208 | m.rotation().transpose() * CartesianAxis3::cross(m.translation()); |
472 |
3/6✓ Branch 3 taken 1104 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1104 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 1104 times.
✗ Branch 10 not taken.
|
2208 | res.template segment<3>(ANGULAR) = m.rotation().transpose().col(axis); |
473 | 2208 | return res; | |
474 | } | ||
475 | |||
476 | ✗ | int nv_impl() const | |
477 | { | ||
478 | ✗ | return NV; | |
479 | } | ||
480 | |||
481 | struct TransposeConst : JointMotionSubspaceTransposeBase<JointMotionSubspaceRevoluteTpl> | ||
482 | { | ||
483 | const JointMotionSubspaceRevoluteTpl & ref; | ||
484 | 12336 | TransposeConst(const JointMotionSubspaceRevoluteTpl & ref) | |
485 | 12336 | : ref(ref) | |
486 | { | ||
487 | 12336 | } | |
488 | |||
489 | template<typename ForceDerived> | ||
490 | typename ConstraintForceOp<JointMotionSubspaceRevoluteTpl, ForceDerived>::ReturnType | ||
491 | 34740 | operator*(const ForceDense<ForceDerived> & f) const | |
492 | { | ||
493 |
1/2✓ Branch 2 taken 17370 times.
✗ Branch 3 not taken.
|
34740 | return f.angular().template segment<1>(axis); |
494 | } | ||
495 | |||
496 | /// [CRBA] MatrixBase operator* (Constraint::Transpose S, ForceSet::Block) | ||
497 | template<typename Derived> | ||
498 | typename ConstraintForceSetOp<JointMotionSubspaceRevoluteTpl, Derived>::ReturnType | ||
499 | 222 | operator*(const Eigen::MatrixBase<Derived> & F) const | |
500 | { | ||
501 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 111 times.
|
222 | assert(F.rows() == 6); |
502 | 222 | return F.row(ANGULAR + axis); | |
503 | } | ||
504 | }; // struct TransposeConst | ||
505 | |||
506 | 12336 | TransposeConst transpose() const | |
507 | { | ||
508 | 12336 | return TransposeConst(*this); | |
509 | } | ||
510 | |||
511 | /* CRBA joint operators | ||
512 | * - ForceSet::Block = ForceSet | ||
513 | * - ForceSet operator* (Inertia Y,Constraint S) | ||
514 | * - MatrixBase operator* (Constraint::Transpose S, ForceSet::Block) | ||
515 | * - SE3::act(ForceSet::Block) | ||
516 | */ | ||
517 | 52 | DenseBase matrix_impl() const | |
518 | { | ||
519 |
1/2✓ Branch 1 taken 26 times.
✗ Branch 2 not taken.
|
52 | DenseBase S; |
520 |
1/2✓ Branch 1 taken 26 times.
✗ Branch 2 not taken.
|
52 | MotionRef<DenseBase> v(S); |
521 |
1/2✓ Branch 1 taken 26 times.
✗ Branch 2 not taken.
|
52 | v << Axis(); |
522 | 104 | return S; | |
523 | } | ||
524 | |||
525 | template<typename MotionDerived> | ||
526 | typename MotionAlgebraAction<JointMotionSubspaceRevoluteTpl, MotionDerived>::ReturnType | ||
527 | 112 | motionAction(const MotionDense<MotionDerived> & m) const | |
528 | { | ||
529 | typedef | ||
530 | typename MotionAlgebraAction<JointMotionSubspaceRevoluteTpl, MotionDerived>::ReturnType | ||
531 | ReturnType; | ||
532 |
1/2✓ Branch 1 taken 56 times.
✗ Branch 2 not taken.
|
112 | ReturnType res; |
533 |
1/2✓ Branch 1 taken 56 times.
✗ Branch 2 not taken.
|
112 | MotionRef<ReturnType> v(res); |
534 |
2/4✓ Branch 1 taken 56 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 56 times.
✗ Branch 5 not taken.
|
112 | v = m.cross(Axis()); |
535 | 224 | return res; | |
536 | } | ||
537 | |||
538 | ✗ | bool isEqual(const JointMotionSubspaceRevoluteTpl &) const | |
539 | { | ||
540 | ✗ | return true; | |
541 | } | ||
542 | |||
543 | }; // struct JointMotionSubspaceRevoluteTpl | ||
544 | |||
545 | template<typename _Scalar, int _Options, int _axis> | ||
546 | struct JointRevoluteTpl | ||
547 | { | ||
548 | typedef _Scalar Scalar; | ||
549 | |||
550 | enum | ||
551 | { | ||
552 | Options = _Options, | ||
553 | axis = _axis | ||
554 | }; | ||
555 | }; | ||
556 | |||
557 | template<typename S1, int O1, typename S2, int O2, int axis> | ||
558 | struct MultiplicationOp<InertiaTpl<S1, O1>, JointMotionSubspaceRevoluteTpl<S2, O2, axis>> | ||
559 | { | ||
560 | typedef Eigen::Matrix<S2, 6, 1, O2> ReturnType; | ||
561 | }; | ||
562 | |||
563 | /* [CRBA] ForceSet operator* (Inertia Y,Constraint S) */ | ||
564 | namespace impl | ||
565 | { | ||
566 | template<typename S1, int O1, typename S2, int O2> | ||
567 | struct LhsMultiplicationOp<InertiaTpl<S1, O1>, JointMotionSubspaceRevoluteTpl<S2, O2, 0>> | ||
568 | { | ||
569 | typedef InertiaTpl<S1, O1> Inertia; | ||
570 | typedef JointMotionSubspaceRevoluteTpl<S2, O2, 0> Constraint; | ||
571 | typedef typename MultiplicationOp<Inertia, Constraint>::ReturnType ReturnType; | ||
572 | ✗ | static inline ReturnType run(const Inertia & Y, const Constraint & /*constraint*/) | |
573 | { | ||
574 | ✗ | ReturnType res; | |
575 | |||
576 | /* Y(:,3) = ( 0,-z, y, I00+yy+zz, I01-xy , I02-xz ) */ | ||
577 | ✗ | const S1 &m = Y.mass(), &x = Y.lever()[0], &y = Y.lever()[1], &z = Y.lever()[2]; | |
578 | ✗ | const typename Inertia::Symmetric3 & I = Y.inertia(); | |
579 | |||
580 | ✗ | res << (S2)0, -m * z, m * y, I(0, 0) + m * (y * y + z * z), I(0, 1) - m * x * y, | |
581 | ✗ | I(0, 2) - m * x * z; | |
582 | |||
583 | ✗ | return res; | |
584 | } | ||
585 | }; | ||
586 | |||
587 | template<typename S1, int O1, typename S2, int O2> | ||
588 | struct LhsMultiplicationOp<InertiaTpl<S1, O1>, JointMotionSubspaceRevoluteTpl<S2, O2, 1>> | ||
589 | { | ||
590 | typedef InertiaTpl<S1, O1> Inertia; | ||
591 | typedef JointMotionSubspaceRevoluteTpl<S2, O2, 1> Constraint; | ||
592 | typedef typename MultiplicationOp<Inertia, Constraint>::ReturnType ReturnType; | ||
593 | ✗ | static inline ReturnType run(const Inertia & Y, const Constraint & /*constraint*/) | |
594 | { | ||
595 | ✗ | ReturnType res; | |
596 | |||
597 | /* Y(:,4) = ( z, 0,-x, I10-xy , I11+xx+zz, I12-yz ) */ | ||
598 | ✗ | const S1 &m = Y.mass(), &x = Y.lever()[0], &y = Y.lever()[1], &z = Y.lever()[2]; | |
599 | ✗ | const typename Inertia::Symmetric3 & I = Y.inertia(); | |
600 | |||
601 | ✗ | res << m * z, (S2)0, -m * x, I(1, 0) - m * x * y, I(1, 1) + m * (x * x + z * z), | |
602 | ✗ | I(1, 2) - m * y * z; | |
603 | |||
604 | ✗ | return res; | |
605 | } | ||
606 | }; | ||
607 | |||
608 | template<typename S1, int O1, typename S2, int O2> | ||
609 | struct LhsMultiplicationOp<InertiaTpl<S1, O1>, JointMotionSubspaceRevoluteTpl<S2, O2, 2>> | ||
610 | { | ||
611 | typedef InertiaTpl<S1, O1> Inertia; | ||
612 | typedef JointMotionSubspaceRevoluteTpl<S2, O2, 2> Constraint; | ||
613 | typedef typename MultiplicationOp<Inertia, Constraint>::ReturnType ReturnType; | ||
614 | ✗ | static inline ReturnType run(const Inertia & Y, const Constraint & /*constraint*/) | |
615 | { | ||
616 | ✗ | ReturnType res; | |
617 | |||
618 | /* Y(:,5) = (-y, x, 0, I20-xz , I21-yz , I22+xx+yy) */ | ||
619 | ✗ | const S1 &m = Y.mass(), &x = Y.lever()[0], &y = Y.lever()[1], &z = Y.lever()[2]; | |
620 | ✗ | const typename Inertia::Symmetric3 & I = Y.inertia(); | |
621 | |||
622 | ✗ | res << -m * y, m * x, (S2)0, I(2, 0) - m * x * z, I(2, 1) - m * y * z, | |
623 | ✗ | I(2, 2) + m * (x * x + y * y); | |
624 | |||
625 | ✗ | return res; | |
626 | } | ||
627 | }; | ||
628 | } // namespace impl | ||
629 | |||
630 | template<typename M6Like, typename S2, int O2, int axis> | ||
631 | struct MultiplicationOp<Eigen::MatrixBase<M6Like>, JointMotionSubspaceRevoluteTpl<S2, O2, axis>> | ||
632 | { | ||
633 | typedef typename M6Like::ConstColXpr ReturnType; | ||
634 | }; | ||
635 | |||
636 | /* [ABA] operator* (Inertia Y,Constraint S) */ | ||
637 | namespace impl | ||
638 | { | ||
639 | template<typename M6Like, typename Scalar, int Options, int axis> | ||
640 | struct LhsMultiplicationOp< | ||
641 | Eigen::MatrixBase<M6Like>, | ||
642 | JointMotionSubspaceRevoluteTpl<Scalar, Options, axis>> | ||
643 | { | ||
644 | typedef JointMotionSubspaceRevoluteTpl<Scalar, Options, axis> Constraint; | ||
645 | typedef | ||
646 | typename MultiplicationOp<Eigen::MatrixBase<M6Like>, Constraint>::ReturnType ReturnType; | ||
647 | static inline ReturnType | ||
648 | ✗ | run(const Eigen::MatrixBase<M6Like> & Y, const Constraint & /*constraint*/) | |
649 | { | ||
650 | EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(M6Like, 6, 6); | ||
651 | ✗ | return Y.col(Inertia::ANGULAR + axis); | |
652 | } | ||
653 | }; | ||
654 | } // namespace impl | ||
655 | |||
656 | template<typename _Scalar, int _Options, int axis> | ||
657 | struct traits<JointRevoluteTpl<_Scalar, _Options, axis>> | ||
658 | { | ||
659 | enum | ||
660 | { | ||
661 | NQ = 1, | ||
662 | NV = 1 | ||
663 | }; | ||
664 | typedef _Scalar Scalar; | ||
665 | enum | ||
666 | { | ||
667 | Options = _Options | ||
668 | }; | ||
669 | typedef JointDataRevoluteTpl<Scalar, Options, axis> JointDataDerived; | ||
670 | typedef JointModelRevoluteTpl<Scalar, Options, axis> JointModelDerived; | ||
671 | typedef JointMotionSubspaceRevoluteTpl<Scalar, Options, axis> Constraint_t; | ||
672 | typedef TransformRevoluteTpl<Scalar, Options, axis> Transformation_t; | ||
673 | typedef MotionRevoluteTpl<Scalar, Options, axis> Motion_t; | ||
674 | typedef MotionZeroTpl<Scalar, Options> Bias_t; | ||
675 | |||
676 | // [ABA] | ||
677 | typedef Eigen::Matrix<Scalar, 6, NV, Options> U_t; | ||
678 | typedef Eigen::Matrix<Scalar, NV, NV, Options> D_t; | ||
679 | typedef Eigen::Matrix<Scalar, 6, NV, Options> UD_t; | ||
680 | |||
681 | typedef Eigen::Matrix<Scalar, NQ, 1, Options> ConfigVector_t; | ||
682 | typedef Eigen::Matrix<Scalar, NV, 1, Options> TangentVector_t; | ||
683 | |||
684 | PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE | ||
685 | }; | ||
686 | |||
687 | template<typename _Scalar, int _Options, int axis> | ||
688 | struct traits<JointDataRevoluteTpl<_Scalar, _Options, axis>> | ||
689 | { | ||
690 | typedef JointRevoluteTpl<_Scalar, _Options, axis> JointDerived; | ||
691 | typedef _Scalar Scalar; | ||
692 | }; | ||
693 | |||
694 | template<typename _Scalar, int _Options, int axis> | ||
695 | struct traits<JointModelRevoluteTpl<_Scalar, _Options, axis>> | ||
696 | { | ||
697 | typedef JointRevoluteTpl<_Scalar, _Options, axis> JointDerived; | ||
698 | typedef _Scalar Scalar; | ||
699 | }; | ||
700 | |||
701 | template<typename _Scalar, int _Options, int axis> | ||
702 | struct JointDataRevoluteTpl : public JointDataBase<JointDataRevoluteTpl<_Scalar, _Options, axis>> | ||
703 | { | ||
704 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW | ||
705 | typedef JointRevoluteTpl<_Scalar, _Options, axis> JointDerived; | ||
706 | PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(JointDerived); | ||
707 | 1753014 | PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR | |
708 | |||
709 | ConfigVector_t joint_q; | ||
710 | TangentVector_t joint_v; | ||
711 | |||
712 | Constraint_t S; | ||
713 | Transformation_t M; | ||
714 | Motion_t v; | ||
715 | Bias_t c; | ||
716 | |||
717 | // [ABA] specific data | ||
718 | U_t U; | ||
719 | D_t Dinv; | ||
720 | UD_t UDinv; | ||
721 | D_t StU; | ||
722 | |||
723 | 30718 | JointDataRevoluteTpl() | |
724 |
1/2✓ Branch 2 taken 15359 times.
✗ Branch 3 not taken.
|
30718 | : joint_q(ConfigVector_t::Zero()) |
725 |
1/2✓ Branch 2 taken 15359 times.
✗ Branch 3 not taken.
|
30718 | , joint_v(TangentVector_t::Zero()) |
726 | 30718 | , M((Scalar)0, (Scalar)1) | |
727 | 30718 | , v((Scalar)0) | |
728 |
1/2✓ Branch 2 taken 15359 times.
✗ Branch 3 not taken.
|
30718 | , U(U_t::Zero()) |
729 |
1/2✓ Branch 2 taken 15359 times.
✗ Branch 3 not taken.
|
30718 | , Dinv(D_t::Zero()) |
730 |
1/2✓ Branch 2 taken 15359 times.
✗ Branch 3 not taken.
|
30718 | , UDinv(UD_t::Zero()) |
731 |
1/2✓ Branch 4 taken 15359 times.
✗ Branch 5 not taken.
|
92154 | , StU(D_t::Zero()) |
732 | { | ||
733 | 30718 | } | |
734 | |||
735 | 480 | static std::string classname() | |
736 | { | ||
737 |
2/4✓ Branch 3 taken 240 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 240 times.
✗ Branch 7 not taken.
|
960 | return std::string("JointDataR") + axisLabel<axis>(); |
738 | } | ||
739 | ✗ | std::string shortname() const | |
740 | { | ||
741 | ✗ | return classname(); | |
742 | } | ||
743 | |||
744 | }; // struct JointDataRevoluteTpl | ||
745 | |||
746 | template<typename NewScalar, typename Scalar, int Options, int axis> | ||
747 | struct CastType<NewScalar, JointModelRevoluteTpl<Scalar, Options, axis>> | ||
748 | { | ||
749 | typedef JointModelRevoluteTpl<NewScalar, Options, axis> type; | ||
750 | }; | ||
751 | |||
752 | template<typename _Scalar, int _Options, int axis> | ||
753 | struct JointModelRevoluteTpl | ||
754 | : public JointModelBase<JointModelRevoluteTpl<_Scalar, _Options, axis>> | ||
755 | { | ||
756 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW | ||
757 | typedef JointRevoluteTpl<_Scalar, _Options, axis> JointDerived; | ||
758 | PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived); | ||
759 | |||
760 | typedef JointModelBase<JointModelRevoluteTpl> Base; | ||
761 | using Base::id; | ||
762 | using Base::idx_q; | ||
763 | using Base::idx_v; | ||
764 | using Base::setIndexes; | ||
765 | |||
766 | typedef Eigen::Matrix<Scalar, 3, 1, _Options> Vector3; | ||
767 | |||
768 | 33472 | JointDataDerived createData() const | |
769 | { | ||
770 | 33472 | return JointDataDerived(); | |
771 | } | ||
772 | |||
773 | 8430 | JointModelRevoluteTpl() | |
774 | 8430 | { | |
775 | 8430 | } | |
776 | |||
777 | ✗ | const std::vector<bool> hasConfigurationLimit() const | |
778 | { | ||
779 | ✗ | return {true}; | |
780 | } | ||
781 | |||
782 | ✗ | const std::vector<bool> hasConfigurationLimitInTangent() const | |
783 | { | ||
784 | ✗ | return {true}; | |
785 | } | ||
786 | |||
787 | template<typename ConfigVector> | ||
788 | EIGEN_DONT_INLINE void | ||
789 | 1138394 | calc(JointDataDerived & data, const typename Eigen::MatrixBase<ConfigVector> & qs) const | |
790 | { | ||
791 |
3/6✓ Branch 1 taken 569197 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 569197 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 569197 times.
✗ Branch 8 not taken.
|
1138394 | data.joint_q[0] = qs[idx_q()]; |
792 | Scalar ca, sa; | ||
793 |
1/2✓ Branch 1 taken 569197 times.
✗ Branch 2 not taken.
|
1138394 | SINCOS(data.joint_q[0], &sa, &ca); |
794 | 1138394 | data.M.setValues(sa, ca); | |
795 | } | ||
796 | |||
797 | template<typename TangentVector> | ||
798 | EIGEN_DONT_INLINE void | ||
799 | ✗ | calc(JointDataDerived & data, const Blank, const typename Eigen::MatrixBase<TangentVector> & vs) | |
800 | const | ||
801 | { | ||
802 | ✗ | data.joint_v[0] = vs[idx_v()]; | |
803 | ✗ | data.v.angularRate() = data.joint_v[0]; | |
804 | } | ||
805 | |||
806 | template<typename ConfigVector, typename TangentVector> | ||
807 | 188074 | EIGEN_DONT_INLINE void calc( | |
808 | JointDataDerived & data, | ||
809 | const typename Eigen::MatrixBase<ConfigVector> & qs, | ||
810 | const typename Eigen::MatrixBase<TangentVector> & vs) const | ||
811 | { | ||
812 | 188074 | calc(data, qs.derived()); | |
813 | |||
814 | 188074 | data.joint_v[0] = vs[idx_v()]; | |
815 | 188074 | data.v.angularRate() = data.joint_v[0]; | |
816 | } | ||
817 | |||
818 | template<typename VectorLike, typename Matrix6Like> | ||
819 | 52 | void calc_aba( | |
820 | JointDataDerived & data, | ||
821 | const Eigen::MatrixBase<VectorLike> & armature, | ||
822 | const Eigen::MatrixBase<Matrix6Like> & I, | ||
823 | const bool update_I) const | ||
824 | { | ||
825 |
1/2✓ Branch 2 taken 26 times.
✗ Branch 3 not taken.
|
52 | data.U = I.col(Inertia::ANGULAR + axis); |
826 | 104 | data.Dinv[0] = | |
827 | 52 | Scalar(1) / (I(Inertia::ANGULAR + axis, Inertia::ANGULAR + axis) + armature[0]); | |
828 |
2/4✓ Branch 3 taken 26 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 26 times.
✗ Branch 7 not taken.
|
52 | data.UDinv.noalias() = data.U * data.Dinv[0]; |
829 | |||
830 |
1/2✓ Branch 0 taken 26 times.
✗ Branch 1 not taken.
|
52 | if (update_I) |
831 |
3/6✓ Branch 2 taken 26 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 26 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 26 times.
✗ Branch 10 not taken.
|
52 | PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like, I).noalias() -= data.UDinv * data.U.transpose(); |
832 | 52 | } | |
833 | |||
834 | 522 | static std::string classname() | |
835 | { | ||
836 |
2/4✓ Branch 3 taken 261 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 261 times.
✗ Branch 7 not taken.
|
1044 | return std::string("JointModelR") + axisLabel<axis>(); |
837 | } | ||
838 | 14 | std::string shortname() const | |
839 | { | ||
840 | 35 | return classname(); | |
841 | } | ||
842 | |||
843 | ✗ | Vector3 getMotionAxis() const | |
844 | { | ||
845 | switch (axis) | ||
846 | { | ||
847 | case 0: | ||
848 | ✗ | return Vector3::UnitX(); | |
849 | case 1: | ||
850 | ✗ | return Vector3::UnitY(); | |
851 | case 2: | ||
852 | ✗ | return Vector3::UnitZ(); | |
853 | default: | ||
854 | assert(false && "must never happen"); | ||
855 | break; | ||
856 | } | ||
857 | } | ||
858 | |||
859 | /// \returns An expression of *this with the Scalar type casted to NewScalar. | ||
860 | template<typename NewScalar> | ||
861 | ✗ | JointModelRevoluteTpl<NewScalar, Options, axis> cast() const | |
862 | { | ||
863 | typedef JointModelRevoluteTpl<NewScalar, Options, axis> ReturnType; | ||
864 | ✗ | ReturnType res; | |
865 | ✗ | res.setIndexes(id(), idx_q(), idx_v()); | |
866 | ✗ | return res; | |
867 | } | ||
868 | |||
869 | }; // struct JointModelRevoluteTpl | ||
870 | |||
871 | typedef JointRevoluteTpl<context::Scalar, context::Options, 0> JointRX; | ||
872 | typedef JointDataRevoluteTpl<context::Scalar, context::Options, 0> JointDataRX; | ||
873 | typedef JointModelRevoluteTpl<context::Scalar, context::Options, 0> JointModelRX; | ||
874 | |||
875 | typedef JointRevoluteTpl<context::Scalar, context::Options, 1> JointRY; | ||
876 | typedef JointDataRevoluteTpl<context::Scalar, context::Options, 1> JointDataRY; | ||
877 | typedef JointModelRevoluteTpl<context::Scalar, context::Options, 1> JointModelRY; | ||
878 | |||
879 | typedef JointRevoluteTpl<context::Scalar, context::Options, 2> JointRZ; | ||
880 | typedef JointDataRevoluteTpl<context::Scalar, context::Options, 2> JointDataRZ; | ||
881 | typedef JointModelRevoluteTpl<context::Scalar, context::Options, 2> JointModelRZ; | ||
882 | |||
883 | } // namespace pinocchio | ||
884 | |||
885 | #include <boost/type_traits.hpp> | ||
886 | |||
887 | namespace boost | ||
888 | { | ||
889 | template<typename Scalar, int Options, int axis> | ||
890 | struct has_nothrow_constructor<::pinocchio::JointModelRevoluteTpl<Scalar, Options, axis>> | ||
891 | : public integral_constant<bool, true> | ||
892 | { | ||
893 | }; | ||
894 | |||
895 | template<typename Scalar, int Options, int axis> | ||
896 | struct has_nothrow_copy<::pinocchio::JointModelRevoluteTpl<Scalar, Options, axis>> | ||
897 | : public integral_constant<bool, true> | ||
898 | { | ||
899 | }; | ||
900 | |||
901 | template<typename Scalar, int Options, int axis> | ||
902 | struct has_nothrow_constructor<::pinocchio::JointDataRevoluteTpl<Scalar, Options, axis>> | ||
903 | : public integral_constant<bool, true> | ||
904 | { | ||
905 | }; | ||
906 | |||
907 | template<typename Scalar, int Options, int axis> | ||
908 | struct has_nothrow_copy<::pinocchio::JointDataRevoluteTpl<Scalar, Options, axis>> | ||
909 | : public integral_constant<bool, true> | ||
910 | { | ||
911 | }; | ||
912 | } // namespace boost | ||
913 | |||
914 | #endif // ifndef __pinocchio_multibody_joint_revolute_hpp__ | ||
915 |