| GCC Code Coverage Report | |||||||||||||||||||||
|
|||||||||||||||||||||
| Line | Branch | Exec | Source |
1 |
// |
||
2 |
// Copyright (c) 2015-2019 CNRS INRIA |
||
3 |
// Copyright (c) 2015-2016 Wandercraft, 86 rue de Paris 91400 Orsay, France. |
||
4 |
// |
||
5 |
|||
6 |
#ifndef __pinocchio_joint_revolute_hpp__ |
||
7 |
#define __pinocchio_joint_revolute_hpp__ |
||
8 |
|||
9 |
#include "pinocchio/math/sincos.hpp" |
||
10 |
#include "pinocchio/spatial/inertia.hpp" |
||
11 |
#include "pinocchio/multibody/constraint.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> struct MotionRevoluteTpl; |
||
20 |
|||
21 |
template<typename Scalar, int Options, int axis> |
||
22 |
struct SE3GroupAction< MotionRevoluteTpl<Scalar,Options,axis> > |
||
23 |
{ |
||
24 |
typedef MotionTpl<Scalar,Options> ReturnType; |
||
25 |
}; |
||
26 |
|||
27 |
template<typename Scalar, int Options, int axis, typename MotionDerived> |
||
28 |
struct MotionAlgebraAction< MotionRevoluteTpl<Scalar,Options,axis>, MotionDerived> |
||
29 |
{ |
||
30 |
typedef MotionTpl<Scalar,Options> ReturnType; |
||
31 |
}; |
||
32 |
|||
33 |
template<typename _Scalar, int _Options, int axis> |
||
34 |
struct traits< MotionRevoluteTpl<_Scalar,_Options,axis> > |
||
35 |
{ |
||
36 |
typedef _Scalar Scalar; |
||
37 |
enum { Options = _Options }; |
||
38 |
typedef Eigen::Matrix<Scalar,3,1,Options> Vector3; |
||
39 |
typedef Eigen::Matrix<Scalar,6,1,Options> Vector6; |
||
40 |
typedef Eigen::Matrix<Scalar,4,4,Options> Matrix4; |
||
41 |
typedef Eigen::Matrix<Scalar,6,6,Options> Matrix6; |
||
42 |
typedef typename PINOCCHIO_EIGEN_REF_CONST_TYPE(Vector6) ToVectorConstReturnType; |
||
43 |
typedef typename PINOCCHIO_EIGEN_REF_TYPE(Vector6) ToVectorReturnType; |
||
44 |
typedef Vector3 AngularType; |
||
45 |
typedef Vector3 LinearType; |
||
46 |
typedef const Vector3 ConstAngularType; |
||
47 |
typedef const Vector3 ConstLinearType; |
||
48 |
typedef Matrix6 ActionMatrixType; |
||
49 |
typedef Matrix4 HomogeneousMatrixType; |
||
50 |
typedef MotionTpl<Scalar,Options> MotionPlain; |
||
51 |
typedef MotionPlain PlainReturnType; |
||
52 |
enum { |
||
53 |
LINEAR = 0, |
||
54 |
ANGULAR = 3 |
||
55 |
}; |
||
56 |
}; // traits MotionRevoluteTpl |
||
57 |
|||
58 |
template<typename Scalar, int Options, int axis> struct TransformRevoluteTpl; |
||
59 |
|||
60 |
template<typename _Scalar, int _Options, int _axis> |
||
61 |
struct traits< TransformRevoluteTpl<_Scalar,_Options,_axis> > |
||
62 |
{ |
||
63 |
enum { |
||
64 |
axis = _axis, |
||
65 |
Options = _Options, |
||
66 |
LINEAR = 0, |
||
67 |
ANGULAR = 3 |
||
68 |
}; |
||
69 |
typedef _Scalar Scalar; |
||
70 |
typedef SE3Tpl<Scalar,Options> PlainType; |
||
71 |
typedef Eigen::Matrix<Scalar,3,1,Options> Vector3; |
||
72 |
typedef Eigen::Matrix<Scalar,3,3,Options> Matrix3; |
||
73 |
typedef Matrix3 AngularType; |
||
74 |
typedef Matrix3 AngularRef; |
||
75 |
typedef Matrix3 ConstAngularRef; |
||
76 |
typedef typename Vector3::ConstantReturnType LinearType; |
||
77 |
typedef typename Vector3::ConstantReturnType LinearRef; |
||
78 |
typedef const typename Vector3::ConstantReturnType ConstLinearRef; |
||
79 |
typedef typename traits<PlainType>::ActionMatrixType ActionMatrixType; |
||
80 |
typedef typename traits<PlainType>::HomogeneousMatrixType HomogeneousMatrixType; |
||
81 |
}; // traits TransformRevoluteTpl |
||
82 |
|||
83 |
template<typename Scalar, int Options, int axis> |
||
84 |
struct SE3GroupAction< TransformRevoluteTpl<Scalar,Options,axis> > |
||
85 |
{ typedef typename traits <TransformRevoluteTpl<Scalar,Options,axis> >::PlainType ReturnType; }; |
||
86 |
|||
87 |
template<typename _Scalar, int _Options, int axis> |
||
88 |
struct TransformRevoluteTpl : SE3Base< TransformRevoluteTpl<_Scalar,_Options,axis> > |
||
89 |
{ |
||
90 |
EIGEN_MAKE_ALIGNED_OPERATOR_NEW |
||
91 |
PINOCCHIO_SE3_TYPEDEF_TPL(TransformRevoluteTpl); |
||
92 |
|||
93 |
126 |
TransformRevoluteTpl() {} |
|
94 |
57878 |
TransformRevoluteTpl(const Scalar & sin, const Scalar & cos) |
|
95 |
58444 |
: m_sin(sin), m_cos(cos) |
|
96 |
58444 |
{} |
|
97 |
|||
98 |
931672 |
PlainType plain() const |
|
99 |
{ |
||
100 |
931672 |
PlainType res(PlainType::Identity()); |
|
101 |
931672 |
_setRotation (res.rotation()); |
|
102 |
931672 |
return res; |
|
103 |
} |
||
104 |
|||
105 |
931672 |
operator PlainType() const { return plain(); } |
|
106 |
|||
107 |
template<typename S2, int O2> |
||
108 |
typename SE3GroupAction<TransformRevoluteTpl>::ReturnType |
||
109 |
se3action(const SE3Tpl<S2,O2> & m) const |
||
110 |
{ |
||
111 |
typedef typename SE3GroupAction<TransformRevoluteTpl>::ReturnType ReturnType; |
||
112 |
ReturnType res; |
||
113 |
switch(axis) |
||
114 |
{ |
||
115 |
case 0: |
||
116 |
{ |
||
117 |
res.rotation().col(0) = m.rotation().col(0); |
||
118 |
res.rotation().col(1).noalias() = m_cos * m.rotation().col(1) + m_sin * m.rotation().col(2); |
||
119 |
res.rotation().col(2).noalias() = res.rotation().col(0).cross(res.rotation().col(1)); |
||
120 |
break; |
||
121 |
} |
||
122 |
case 1: |
||
123 |
{ |
||
124 |
res.rotation().col(2).noalias() = m_cos * m.rotation().col(2) + m_sin * m.rotation().col(0); |
||
125 |
res.rotation().col(1) = m.rotation().col(1); |
||
126 |
res.rotation().col(0).noalias() = res.rotation().col(1).cross(res.rotation().col(2)); |
||
127 |
break; |
||
128 |
} |
||
129 |
case 2: |
||
130 |
{ |
||
131 |
res.rotation().col(0).noalias() = m_cos * m.rotation().col(0) + m_sin * m.rotation().col(1); |
||
132 |
res.rotation().col(1).noalias() = res.rotation().col(2).cross(res.rotation().col(0)); |
||
133 |
res.rotation().col(2) = m.rotation().col(2); |
||
134 |
break; |
||
135 |
} |
||
136 |
default: |
||
137 |
{ |
||
138 |
assert(false && "must nerver happened"); |
||
139 |
break; |
||
140 |
} |
||
141 |
} |
||
142 |
res.translation() = m.translation(); |
||
143 |
return res; |
||
144 |
} |
||
145 |
|||
146 |
const Scalar & sin() const { return m_sin; } |
||
147 |
1400 |
Scalar & sin() { return m_sin; } |
|
148 |
|||
149 |
const Scalar & cos() const { return m_cos; } |
||
150 |
1400 |
Scalar & cos() { return m_cos; } |
|
151 |
|||
152 |
template<typename OtherScalar> |
||
153 |
930526 |
void setValues(const OtherScalar & sin, const OtherScalar & cos) |
|
154 |
930526 |
{ m_sin = sin; m_cos = cos; } |
|
155 |
|||
156 |
LinearType translation() const { return LinearType::PlainObject::Zero(3); }; |
||
157 |
AngularType rotation() const { |
||
158 |
AngularType m(AngularType::Identity(3)); |
||
159 |
_setRotation (m); |
||
160 |
return m; |
||
161 |
} |
||
162 |
|||
163 |
894 |
bool isEqual(const TransformRevoluteTpl & other) const |
|
164 |
{ |
||
165 |
✓✗✓✗ |
894 |
return m_cos == other.m_cos && m_sin == other.m_sin; |
166 |
} |
||
167 |
|||
168 |
protected: |
||
169 |
|||
170 |
Scalar m_sin, m_cos; |
||
171 |
931672 |
inline void _setRotation (typename PlainType::AngularRef& rot) const |
|
172 |
{ |
||
173 |
switch(axis) |
||
174 |
{ |
||
175 |
case 0: |
||
176 |
{ |
||
177 |
288908 |
rot.coeffRef(1,1) = m_cos; rot.coeffRef(1,2) = -m_sin; |
|
178 |
288908 |
rot.coeffRef(2,1) = m_sin; rot.coeffRef(2,2) = m_cos; |
|
179 |
288908 |
break; |
|
180 |
} |
||
181 |
case 1: |
||
182 |
{ |
||
183 |
453366 |
rot.coeffRef(0,0) = m_cos; rot.coeffRef(0,2) = m_sin; |
|
184 |
453366 |
rot.coeffRef(2,0) = -m_sin; rot.coeffRef(2,2) = m_cos; |
|
185 |
453366 |
break; |
|
186 |
} |
||
187 |
case 2: |
||
188 |
{ |
||
189 |
189398 |
rot.coeffRef(0,0) = m_cos; rot.coeffRef(0,1) = -m_sin; |
|
190 |
189398 |
rot.coeffRef(1,0) = m_sin; rot.coeffRef(1,1) = m_cos; |
|
191 |
189398 |
break; |
|
192 |
} |
||
193 |
default: |
||
194 |
{ |
||
195 |
assert(false && "must nerver happened"); |
||
196 |
break; |
||
197 |
} |
||
198 |
} |
||
199 |
931672 |
} |
|
200 |
}; |
||
201 |
|||
202 |
template<typename _Scalar, int _Options, int axis> |
||
203 |
struct MotionRevoluteTpl |
||
204 |
: MotionBase< MotionRevoluteTpl<_Scalar,_Options,axis> > |
||
205 |
{ |
||
206 |
EIGEN_MAKE_ALIGNED_OPERATOR_NEW |
||
207 |
|||
208 |
MOTION_TYPEDEF_TPL(MotionRevoluteTpl); |
||
209 |
typedef SpatialAxis<axis+ANGULAR> Axis; |
||
210 |
typedef typename Axis::CartesianAxis3 CartesianAxis3; |
||
211 |
|||
212 |
126 |
MotionRevoluteTpl() {} |
|
213 |
|||
214 |
121434 |
MotionRevoluteTpl(const Scalar & w) : m_w(w) {} |
|
215 |
|||
216 |
template<typename Vector1Like> |
||
217 |
MotionRevoluteTpl(const Eigen::MatrixBase<Vector1Like> & v) |
||
218 |
: m_w(v[0]) |
||
219 |
{ |
||
220 |
using namespace Eigen; |
||
221 |
EIGEN_STATIC_ASSERT_SIZE_1x1(Vector1Like); |
||
222 |
} |
||
223 |
|||
224 |
✓✗ | 30 |
inline PlainReturnType plain() const { return Axis() * m_w; } |
225 |
|||
226 |
template<typename OtherScalar> |
||
227 |
18 |
MotionRevoluteTpl __mult__(const OtherScalar & alpha) const |
|
228 |
{ |
||
229 |
18 |
return MotionRevoluteTpl(alpha*m_w); |
|
230 |
} |
||
231 |
|||
232 |
template<typename MotionDerived> |
||
233 |
99084 |
void setTo(MotionDense<MotionDerived> & m) const |
|
234 |
{ |
||
235 |
✓✗ | 99084 |
m.linear().setZero(); |
236 |
✓✓ | 396336 |
for(Eigen::DenseIndex k = 0; k < 3; ++k) |
237 |
✓✓✓✗ |
297252 |
m.angular()[k] = k == axis ? m_w : (Scalar)0; |
238 |
99084 |
} |
|
239 |
|||
240 |
template<typename MotionDerived> |
||
241 |
62716 |
inline void addTo(MotionDense<MotionDerived> & v) const |
|
242 |
{ |
||
243 |
typedef typename MotionDense<MotionDerived>::Scalar OtherScalar; |
||
244 |
✓✗ | 62716 |
v.angular()[axis] += (OtherScalar)m_w; |
245 |
62716 |
} |
|
246 |
|||
247 |
template<typename S2, int O2, typename D2> |
||
248 |
428 |
inline void se3Action_impl(const SE3Tpl<S2,O2> & m, MotionDense<D2> & v) const |
|
249 |
{ |
||
250 |
✓✗✓✗ ✓✗✓✗ |
428 |
v.angular().noalias() = m.rotation().col(axis) * m_w; |
251 |
✓✗✓✗ ✓✗✓✗ |
428 |
v.linear().noalias() = m.translation().cross(v.angular()); |
252 |
428 |
} |
|
253 |
|||
254 |
template<typename S2, int O2> |
||
255 |
428 |
MotionPlain se3Action_impl(const SE3Tpl<S2,O2> & m) const |
|
256 |
{ |
||
257 |
428 |
MotionPlain res; |
|
258 |
428 |
se3Action_impl(m,res); |
|
259 |
428 |
return res; |
|
260 |
} |
||
261 |
|||
262 |
template<typename S2, int O2, typename D2> |
||
263 |
60 |
void se3ActionInverse_impl(const SE3Tpl<S2,O2> & m, |
|
264 |
MotionDense<D2> & v) const |
||
265 |
{ |
||
266 |
// Linear |
||
267 |
✓✗✓✗ |
60 |
CartesianAxis3::alphaCross(m_w,m.translation(),v.angular()); |
268 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
60 |
v.linear().noalias() = m.rotation().transpose() * v.angular(); |
269 |
|||
270 |
// Angular |
||
271 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
60 |
v.angular().noalias() = m.rotation().transpose().col(axis) * m_w; |
272 |
60 |
} |
|
273 |
|||
274 |
template<typename S2, int O2> |
||
275 |
60 |
MotionPlain se3ActionInverse_impl(const SE3Tpl<S2,O2> & m) const |
|
276 |
{ |
||
277 |
60 |
MotionPlain res; |
|
278 |
60 |
se3ActionInverse_impl(m,res); |
|
279 |
60 |
return res; |
|
280 |
} |
||
281 |
|||
282 |
template<typename M1, typename M2> |
||
283 |
EIGEN_STRONG_INLINE |
||
284 |
73892 |
void motionAction(const MotionDense<M1> & v, MotionDense<M2> & mout) const |
|
285 |
{ |
||
286 |
// Linear |
||
287 |
✓✗✓✗ |
73892 |
CartesianAxis3::alphaCross(-m_w,v.linear(),mout.linear()); |
288 |
|||
289 |
// Angular |
||
290 |
✓✗✓✗ |
73892 |
CartesianAxis3::alphaCross(-m_w,v.angular(),mout.angular()); |
291 |
73892 |
} |
|
292 |
|||
293 |
template<typename M1> |
||
294 |
73892 |
MotionPlain motionAction(const MotionDense<M1> & v) const |
|
295 |
{ |
||
296 |
73892 |
MotionPlain res; |
|
297 |
73892 |
motionAction(v,res); |
|
298 |
73892 |
return res; |
|
299 |
} |
||
300 |
|||
301 |
99566 |
Scalar & angularRate() { return m_w; } |
|
302 |
const Scalar & angularRate() const { return m_w; } |
||
303 |
|||
304 |
894 |
bool isEqual_impl(const MotionRevoluteTpl & other) const |
|
305 |
{ |
||
306 |
894 |
return m_w == other.m_w; |
|
307 |
} |
||
308 |
|||
309 |
protected: |
||
310 |
|||
311 |
Scalar m_w; |
||
312 |
}; // struct MotionRevoluteTpl |
||
313 |
|||
314 |
template<typename S1, int O1, int axis, typename MotionDerived> |
||
315 |
typename MotionDerived::MotionPlain |
||
316 |
31980 |
operator+(const MotionRevoluteTpl<S1,O1,axis> & m1, |
|
317 |
const MotionDense<MotionDerived> & m2) |
||
318 |
{ |
||
319 |
31980 |
typename MotionDerived::MotionPlain res(m2); |
|
320 |
31980 |
res += m1; |
|
321 |
31980 |
return res; |
|
322 |
} |
||
323 |
|||
324 |
template<typename MotionDerived, typename S2, int O2, int axis> |
||
325 |
EIGEN_STRONG_INLINE |
||
326 |
typename MotionDerived::MotionPlain |
||
327 |
73880 |
operator^(const MotionDense<MotionDerived> & m1, const MotionRevoluteTpl<S2,O2,axis>& m2) |
|
328 |
{ |
||
329 |
73880 |
return m2.motionAction(m1); |
|
330 |
} |
||
331 |
|||
332 |
template<typename Scalar, int Options, int axis> struct ConstraintRevoluteTpl; |
||
333 |
|||
334 |
template<typename Scalar, int Options, int axis> |
||
335 |
struct SE3GroupAction< ConstraintRevoluteTpl<Scalar,Options,axis> > |
||
336 |
{ typedef Eigen::Matrix<Scalar,6,1,Options> ReturnType; }; |
||
337 |
|||
338 |
template<typename Scalar, int Options, int axis, typename MotionDerived> |
||
339 |
struct MotionAlgebraAction< ConstraintRevoluteTpl<Scalar,Options,axis>, MotionDerived > |
||
340 |
{ typedef Eigen::Matrix<Scalar,6,1,Options> ReturnType; }; |
||
341 |
|||
342 |
template<typename Scalar, int Options, int axis, typename ForceDerived> |
||
343 |
struct ConstraintForceOp< ConstraintRevoluteTpl<Scalar,Options,axis>, ForceDerived> |
||
344 |
{ typedef typename ForceDense<ForceDerived>::ConstAngularType::template ConstFixedSegmentReturnType<1>::Type ReturnType; }; |
||
345 |
|||
346 |
template<typename Scalar, int Options, int axis, typename ForceSet> |
||
347 |
struct ConstraintForceSetOp< ConstraintRevoluteTpl<Scalar,Options,axis>, ForceSet> |
||
348 |
{ typedef typename Eigen::MatrixBase<ForceSet>::ConstRowXpr ReturnType; }; |
||
349 |
|||
350 |
template<typename _Scalar, int _Options, int axis> |
||
351 |
struct traits< ConstraintRevoluteTpl<_Scalar,_Options,axis> > |
||
352 |
{ |
||
353 |
typedef _Scalar Scalar; |
||
354 |
enum { Options = _Options }; |
||
355 |
enum { |
||
356 |
LINEAR = 0, |
||
357 |
ANGULAR = 3 |
||
358 |
}; |
||
359 |
typedef MotionRevoluteTpl<Scalar,Options,axis> JointMotion; |
||
360 |
typedef Eigen::Matrix<Scalar,1,1,Options> JointForce; |
||
361 |
typedef Eigen::Matrix<Scalar,6,1,Options> DenseBase; |
||
362 |
typedef DenseBase MatrixReturnType; |
||
363 |
typedef const DenseBase ConstMatrixReturnType; |
||
364 |
}; // traits ConstraintRevoluteTpl |
||
365 |
|||
366 |
template<typename _Scalar, int _Options, int axis> |
||
367 |
struct ConstraintRevoluteTpl |
||
368 |
: ConstraintBase< ConstraintRevoluteTpl<_Scalar,_Options,axis> > |
||
369 |
{ |
||
370 |
EIGEN_MAKE_ALIGNED_OPERATOR_NEW |
||
371 |
|||
372 |
PINOCCHIO_CONSTRAINT_TYPEDEF_TPL(ConstraintRevoluteTpl) |
||
373 |
enum { NV = 1 }; |
||
374 |
|||
375 |
typedef SpatialAxis<ANGULAR+axis> Axis; |
||
376 |
|||
377 |
58908 |
ConstraintRevoluteTpl() {} |
|
378 |
|||
379 |
template<typename Vector1Like> |
||
380 |
62966 |
JointMotion __mult__(const Eigen::MatrixBase<Vector1Like> & v) const |
|
381 |
62966 |
{ return JointMotion(v[0]); } |
|
382 |
|||
383 |
template<typename S1, int O1> |
||
384 |
typename SE3GroupAction<ConstraintRevoluteTpl>::ReturnType |
||
385 |
42218 |
se3Action(const SE3Tpl<S1,O1> & m) const |
|
386 |
{ |
||
387 |
typedef typename SE3GroupAction<ConstraintRevoluteTpl>::ReturnType ReturnType; |
||
388 |
42218 |
ReturnType res; |
|
389 |
✓✗✓✗ ✓✗ |
42218 |
res.template segment<3>(LINEAR) = m.translation().cross(m.rotation().col(axis)); |
390 |
✓✗✓✗ |
42218 |
res.template segment<3>(ANGULAR) = m.rotation().col(axis); |
391 |
42218 |
return res; |
|
392 |
} |
||
393 |
|||
394 |
template<typename S1, int O1> |
||
395 |
typename SE3GroupAction<ConstraintRevoluteTpl>::ReturnType |
||
396 |
2372 |
se3ActionInverse(const SE3Tpl<S1,O1> & m) const |
|
397 |
{ |
||
398 |
typedef typename SE3GroupAction<ConstraintRevoluteTpl>::ReturnType ReturnType; |
||
399 |
typedef typename Axis::CartesianAxis3 CartesianAxis3; |
||
400 |
2372 |
ReturnType res; |
|
401 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
2372 |
res.template segment<3>(LINEAR).noalias() = m.rotation().transpose()*CartesianAxis3::cross(m.translation()); |
402 |
✓✗✓✗ ✓✗ |
2372 |
res.template segment<3>(ANGULAR) = m.rotation().transpose().col(axis); |
403 |
2372 |
return res; |
|
404 |
} |
||
405 |
|||
406 |
330 |
int nv_impl() const { return NV; } |
|
407 |
|||
408 |
struct TransposeConst |
||
409 |
{ |
||
410 |
const ConstraintRevoluteTpl & ref; |
||
411 |
47628 |
TransposeConst(const ConstraintRevoluteTpl & ref) : ref(ref) {} |
|
412 |
|||
413 |
template<typename ForceDerived> |
||
414 |
typename ConstraintForceOp<ConstraintRevoluteTpl,ForceDerived>::ReturnType |
||
415 |
45634 |
operator*(const ForceDense<ForceDerived> & f) const |
|
416 |
✓✗ | 45634 |
{ return f.angular().template segment<1>(axis); } |
417 |
|||
418 |
/// [CRBA] MatrixBase operator* (Constraint::Transpose S, ForceSet::Block) |
||
419 |
template<typename Derived> |
||
420 |
typename ConstraintForceSetOp<ConstraintRevoluteTpl,Derived>::ReturnType |
||
421 |
1994 |
operator*(const Eigen::MatrixBase<Derived> & F) const |
|
422 |
{ |
||
423 |
✗✓ | 1994 |
assert(F.rows()==6); |
424 |
1994 |
return F.row(ANGULAR + axis); |
|
425 |
} |
||
426 |
}; // struct TransposeConst |
||
427 |
|||
428 |
47628 |
TransposeConst transpose() const { return TransposeConst(*this); } |
|
429 |
|||
430 |
/* CRBA joint operators |
||
431 |
* - ForceSet::Block = ForceSet |
||
432 |
* - ForceSet operator* (Inertia Y,Constraint S) |
||
433 |
* - MatrixBase operator* (Constraint::Transpose S, ForceSet::Block) |
||
434 |
* - SE3::act(ForceSet::Block) |
||
435 |
*/ |
||
436 |
1506 |
DenseBase matrix_impl() const |
|
437 |
{ |
||
438 |
✓✗ | 1506 |
DenseBase S; |
439 |
✓✗ | 1506 |
MotionRef<DenseBase> v(S); |
440 |
✓✗ | 1506 |
v << Axis(); |
441 |
3012 |
return S; |
|
442 |
} |
||
443 |
|||
444 |
template<typename MotionDerived> |
||
445 |
typename MotionAlgebraAction<ConstraintRevoluteTpl,MotionDerived>::ReturnType |
||
446 |
172 |
motionAction(const MotionDense<MotionDerived> & m) const |
|
447 |
{ |
||
448 |
typedef typename MotionAlgebraAction<ConstraintRevoluteTpl,MotionDerived>::ReturnType ReturnType; |
||
449 |
✓✗ | 172 |
ReturnType res; |
450 |
✓✗ | 172 |
MotionRef<ReturnType> v(res); |
451 |
✓✗✓✗ |
172 |
v = m.cross(Axis()); |
452 |
344 |
return res; |
|
453 |
} |
||
454 |
|||
455 |
894 |
bool isEqual(const ConstraintRevoluteTpl &) const { return true; } |
|
456 |
|||
457 |
}; // struct ConstraintRevoluteTpl |
||
458 |
|||
459 |
template<typename _Scalar, int _Options, int _axis> |
||
460 |
struct JointRevoluteTpl |
||
461 |
{ |
||
462 |
typedef _Scalar Scalar; |
||
463 |
|||
464 |
enum |
||
465 |
{ |
||
466 |
Options = _Options, |
||
467 |
axis = _axis |
||
468 |
}; |
||
469 |
}; |
||
470 |
|||
471 |
template<typename S1, int O1,typename S2, int O2, int axis> |
||
472 |
struct MultiplicationOp<InertiaTpl<S1,O1>, ConstraintRevoluteTpl<S2,O2,axis> > |
||
473 |
{ |
||
474 |
typedef Eigen::Matrix<S2,6,1,O2> ReturnType; |
||
475 |
}; |
||
476 |
|||
477 |
/* [CRBA] ForceSet operator* (Inertia Y,Constraint S) */ |
||
478 |
namespace impl |
||
479 |
{ |
||
480 |
template<typename S1, int O1, typename S2, int O2> |
||
481 |
struct LhsMultiplicationOp<InertiaTpl<S1,O1>, ConstraintRevoluteTpl<S2,O2,0> > |
||
482 |
{ |
||
483 |
typedef InertiaTpl<S1,O1> Inertia; |
||
484 |
typedef ConstraintRevoluteTpl<S2,O2,0> Constraint; |
||
485 |
typedef typename MultiplicationOp<Inertia,Constraint>::ReturnType ReturnType; |
||
486 |
297 |
static inline ReturnType run(const Inertia & Y, |
|
487 |
const Constraint & /*constraint*/) |
||
488 |
{ |
||
489 |
297 |
ReturnType res; |
|
490 |
|||
491 |
/* Y(:,3) = ( 0,-z, y, I00+yy+zz, I01-xy , I02-xz ) */ |
||
492 |
const S1 |
||
493 |
297 |
&m = Y.mass(), |
|
494 |
297 |
&x = Y.lever()[0], |
|
495 |
297 |
&y = Y.lever()[1], |
|
496 |
297 |
&z = Y.lever()[2]; |
|
497 |
297 |
const typename Inertia::Symmetric3 & I = Y.inertia(); |
|
498 |
|||
499 |
res << |
||
500 |
✓✗ | 297 |
(S2)0, |
501 |
✓✗ | 297 |
-m*z, |
502 |
✓✗ | 297 |
m*y, |
503 |
✓✗✓✗ |
297 |
I(0,0)+m*(y*y+z*z), |
504 |
✓✗✓✗ |
297 |
I(0,1)-m*x*y, |
505 |
✓✗✓✗ |
297 |
I(0,2)-m*x*z; |
506 |
|||
507 |
297 |
return res; |
|
508 |
} |
||
509 |
}; |
||
510 |
|||
511 |
template<typename S1, int O1, typename S2, int O2> |
||
512 |
struct LhsMultiplicationOp<InertiaTpl<S1,O1>, ConstraintRevoluteTpl<S2,O2,1> > |
||
513 |
{ |
||
514 |
typedef InertiaTpl<S1,O1> Inertia; |
||
515 |
typedef ConstraintRevoluteTpl<S2,O2,1> Constraint; |
||
516 |
typedef typename MultiplicationOp<Inertia,Constraint>::ReturnType ReturnType; |
||
517 |
466 |
static inline ReturnType run(const Inertia & Y, |
|
518 |
const Constraint & /*constraint*/) |
||
519 |
{ |
||
520 |
466 |
ReturnType res; |
|
521 |
|||
522 |
/* Y(:,4) = ( z, 0,-x, I10-xy , I11+xx+zz, I12-yz ) */ |
||
523 |
const S1 |
||
524 |
466 |
&m = Y.mass(), |
|
525 |
466 |
&x = Y.lever()[0], |
|
526 |
466 |
&y = Y.lever()[1], |
|
527 |
466 |
&z = Y.lever()[2]; |
|
528 |
466 |
const typename Inertia::Symmetric3 & I = Y.inertia(); |
|
529 |
|||
530 |
res << |
||
531 |
✓✗✓✗ |
466 |
m*z, |
532 |
932 |
(S2)0, |
|
533 |
✓✗ | 466 |
-m*x, |
534 |
✓✗✓✗ |
466 |
I(1,0)-m*x*y, |
535 |
✓✗✓✗ |
466 |
I(1,1)+m*(x*x+z*z), |
536 |
✓✗✓✗ |
466 |
I(1,2)-m*y*z; |
537 |
|||
538 |
466 |
return res; |
|
539 |
} |
||
540 |
}; |
||
541 |
|||
542 |
template<typename S1, int O1, typename S2, int O2> |
||
543 |
struct LhsMultiplicationOp<InertiaTpl<S1,O1>, ConstraintRevoluteTpl<S2,O2,2> > |
||
544 |
{ |
||
545 |
typedef InertiaTpl<S1,O1> Inertia; |
||
546 |
typedef ConstraintRevoluteTpl<S2,O2,2> Constraint; |
||
547 |
typedef typename MultiplicationOp<Inertia,Constraint>::ReturnType ReturnType; |
||
548 |
184 |
static inline ReturnType run(const Inertia & Y, |
|
549 |
const Constraint & /*constraint*/) |
||
550 |
{ |
||
551 |
184 |
ReturnType res; |
|
552 |
|||
553 |
/* Y(:,5) = (-y, x, 0, I20-xz , I21-yz , I22+xx+yy) */ |
||
554 |
const S1 |
||
555 |
184 |
&m = Y.mass(), |
|
556 |
184 |
&x = Y.lever()[0], |
|
557 |
184 |
&y = Y.lever()[1], |
|
558 |
184 |
&z = Y.lever()[2]; |
|
559 |
184 |
const typename Inertia::Symmetric3 & I = Y.inertia(); |
|
560 |
|||
561 |
res << |
||
562 |
✓✗ | 184 |
-m*y, |
563 |
✓✗✓✗ |
184 |
m*x, |
564 |
368 |
(S2)0, |
|
565 |
✓✗✓✗ |
184 |
I(2,0)-m*x*z, |
566 |
✓✗✓✗ |
184 |
I(2,1)-m*y*z, |
567 |
✓✗✓✗ |
184 |
I(2,2)+m*(x*x+y*y); |
568 |
|||
569 |
184 |
return res; |
|
570 |
} |
||
571 |
}; |
||
572 |
} // namespace impl |
||
573 |
|||
574 |
template<typename M6Like,typename S2, int O2, int axis> |
||
575 |
struct MultiplicationOp<Eigen::MatrixBase<M6Like>, ConstraintRevoluteTpl<S2,O2,axis> > |
||
576 |
{ |
||
577 |
typedef typename M6Like::ConstColXpr ReturnType; |
||
578 |
}; |
||
579 |
|||
580 |
/* [ABA] operator* (Inertia Y,Constraint S) */ |
||
581 |
namespace impl |
||
582 |
{ |
||
583 |
template<typename M6Like, typename Scalar, int Options, int axis> |
||
584 |
struct LhsMultiplicationOp<Eigen::MatrixBase<M6Like>, ConstraintRevoluteTpl<Scalar,Options,axis> > |
||
585 |
{ |
||
586 |
typedef ConstraintRevoluteTpl<Scalar,Options,axis> Constraint; |
||
587 |
typedef typename MultiplicationOp<Eigen::MatrixBase<M6Like>,Constraint>::ReturnType ReturnType; |
||
588 |
36 |
static inline ReturnType run(const Eigen::MatrixBase<M6Like> & Y, |
|
589 |
const Constraint & /*constraint*/) |
||
590 |
{ |
||
591 |
EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(M6Like,6,6); |
||
592 |
36 |
return Y.col(Inertia::ANGULAR + axis); |
|
593 |
} |
||
594 |
}; |
||
595 |
} // namespace impl |
||
596 |
|||
597 |
template<typename _Scalar, int _Options, int axis> |
||
598 |
struct traits< JointRevoluteTpl<_Scalar,_Options,axis> > |
||
599 |
{ |
||
600 |
enum { |
||
601 |
NQ = 1, |
||
602 |
NV = 1 |
||
603 |
}; |
||
604 |
typedef _Scalar Scalar; |
||
605 |
enum { Options = _Options }; |
||
606 |
typedef JointDataRevoluteTpl<Scalar,Options,axis> JointDataDerived; |
||
607 |
typedef JointModelRevoluteTpl<Scalar,Options,axis> JointModelDerived; |
||
608 |
typedef ConstraintRevoluteTpl<Scalar,Options,axis> Constraint_t; |
||
609 |
typedef TransformRevoluteTpl<Scalar,Options,axis> Transformation_t; |
||
610 |
typedef MotionRevoluteTpl<Scalar,Options,axis> Motion_t; |
||
611 |
typedef MotionZeroTpl<Scalar,Options> Bias_t; |
||
612 |
|||
613 |
// [ABA] |
||
614 |
typedef Eigen::Matrix<Scalar,6,NV,Options> U_t; |
||
615 |
typedef Eigen::Matrix<Scalar,NV,NV,Options> D_t; |
||
616 |
typedef Eigen::Matrix<Scalar,6,NV,Options> UD_t; |
||
617 |
|||
618 |
PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE |
||
619 |
|||
620 |
typedef Eigen::Matrix<Scalar,NQ,1,Options> ConfigVector_t; |
||
621 |
typedef Eigen::Matrix<Scalar,NV,1,Options> TangentVector_t; |
||
622 |
}; |
||
623 |
|||
624 |
template<typename Scalar, int Options, int axis> |
||
625 |
struct traits< JointDataRevoluteTpl<Scalar,Options,axis> > |
||
626 |
{ typedef JointRevoluteTpl<Scalar,Options,axis> JointDerived; }; |
||
627 |
|||
628 |
template<typename Scalar, int Options, int axis> |
||
629 |
struct traits< JointModelRevoluteTpl<Scalar,Options,axis> > |
||
630 |
{ typedef JointRevoluteTpl<Scalar,Options,axis> JointDerived; }; |
||
631 |
|||
632 |
template<typename _Scalar, int _Options, int axis> |
||
633 |
struct JointDataRevoluteTpl : public JointDataBase< JointDataRevoluteTpl<_Scalar,_Options,axis> > |
||
634 |
{ |
||
635 |
62654 |
EIGEN_MAKE_ALIGNED_OPERATOR_NEW |
|
636 |
typedef JointRevoluteTpl<_Scalar,_Options,axis> JointDerived; |
||
637 |
PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(JointDerived); |
||
638 |
1318206 |
PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR |
|
639 |
|||
640 |
Constraint_t S; |
||
641 |
Transformation_t M; |
||
642 |
Motion_t v; |
||
643 |
Bias_t c; |
||
644 |
|||
645 |
// [ABA] specific data |
||
646 |
U_t U; |
||
647 |
D_t Dinv; |
||
648 |
UD_t UDinv; |
||
649 |
|||
650 |
39696 |
JointDataRevoluteTpl() |
|
651 |
: M((Scalar)0,(Scalar)1) |
||
652 |
, v((Scalar)0) |
||
653 |
, U(U_t::Zero()) |
||
654 |
, Dinv(D_t::Zero()) |
||
655 |
✓✗✓✗ ✓✗ |
39696 |
, UDinv(UD_t::Zero()) |
656 |
39696 |
{} |
|
657 |
|||
658 |
530 |
static std::string classname() |
|
659 |
{ |
||
660 |
✓✗✓✗ |
530 |
return std::string("JointDataR") + axisLabel<axis>(); |
661 |
} |
||
662 |
19 |
std::string shortname() const { return classname(); } |
|
663 |
|||
664 |
}; // struct JointDataRevoluteTpl |
||
665 |
|||
666 |
template<typename NewScalar, typename Scalar, int Options, int axis> |
||
667 |
struct CastType< NewScalar, JointModelRevoluteTpl<Scalar,Options,axis> > |
||
668 |
{ |
||
669 |
typedef JointModelRevoluteTpl<NewScalar,Options,axis> type; |
||
670 |
}; |
||
671 |
|||
672 |
template<typename _Scalar, int _Options, int axis> |
||
673 |
struct JointModelRevoluteTpl |
||
674 |
: public JointModelBase< JointModelRevoluteTpl<_Scalar,_Options,axis> > |
||
675 |
{ |
||
676 |
80754 |
EIGEN_MAKE_ALIGNED_OPERATOR_NEW |
|
677 |
typedef JointRevoluteTpl<_Scalar,_Options,axis> JointDerived; |
||
678 |
PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived); |
||
679 |
|||
680 |
typedef JointModelBase<JointModelRevoluteTpl> Base; |
||
681 |
using Base::id; |
||
682 |
using Base::idx_q; |
||
683 |
using Base::idx_v; |
||
684 |
using Base::setIndexes; |
||
685 |
|||
686 |
37504 |
JointDataDerived createData() const { return JointDataDerived(); } |
|
687 |
|||
688 |
17875 |
JointModelRevoluteTpl() {} |
|
689 |
|||
690 |
2 |
const std::vector<bool> hasConfigurationLimit() const |
|
691 |
{ |
||
692 |
✓✗ | 2 |
return {true}; |
693 |
} |
||
694 |
|||
695 |
2 |
const std::vector<bool> hasConfigurationLimitInTangent() const |
|
696 |
{ |
||
697 |
✓✗ | 2 |
return {true}; |
698 |
} |
||
699 |
|||
700 |
template<typename ConfigVector> |
||
701 |
EIGEN_DONT_INLINE |
||
702 |
893662 |
void calc(JointDataDerived & data, |
|
703 |
const typename Eigen::MatrixBase<ConfigVector> & qs) const |
||
704 |
{ |
||
705 |
typedef typename ConfigVector::Scalar OtherScalar; |
||
706 |
|||
707 |
✓✗✓✗ |
893662 |
const OtherScalar & q = qs[idx_q()]; |
708 |
893662 |
OtherScalar ca,sa; SINCOS(q,&sa,&ca); |
|
709 |
893662 |
data.M.setValues(sa,ca); |
|
710 |
893662 |
} |
|
711 |
|||
712 |
template<typename ConfigVector, typename TangentVector> |
||
713 |
EIGEN_DONT_INLINE |
||
714 |
91964 |
void calc(JointDataDerived & data, |
|
715 |
const typename Eigen::MatrixBase<ConfigVector> & qs, |
||
716 |
const typename Eigen::MatrixBase<TangentVector> & vs) const |
||
717 |
{ |
||
718 |
91964 |
calc(data,qs.derived()); |
|
719 |
|||
720 |
91964 |
data.v.angularRate() = static_cast<Scalar>(vs[idx_v()]); |
|
721 |
91964 |
} |
|
722 |
|||
723 |
template<typename Matrix6Like> |
||
724 |
13290 |
void calc_aba(JointDataDerived & data, |
|
725 |
const Eigen::MatrixBase<Matrix6Like> & I, |
||
726 |
const bool update_I) const |
||
727 |
{ |
||
728 |
✓✗ | 13290 |
data.U = I.col(Inertia::ANGULAR + axis); |
729 |
13290 |
data.Dinv[0] = Scalar(1)/I(Inertia::ANGULAR + axis,Inertia::ANGULAR + axis); |
|
730 |
✓✗✓✗ |
13290 |
data.UDinv.noalias() = data.U * data.Dinv[0]; |
731 |
|||
732 |
✓✓ | 13290 |
if (update_I) |
733 |
✓✗✓✗ |
13230 |
PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like,I) -= data.UDinv * data.U.transpose(); |
734 |
13290 |
} |
|
735 |
|||
736 |
110998 |
static std::string classname() |
|
737 |
{ |
||
738 |
✓✗✓✗ |
110998 |
return std::string("JointModelR") + axisLabel<axis>(); |
739 |
} |
||
740 |
55244 |
std::string shortname() const { return classname(); } |
|
741 |
|||
742 |
/// \returns An expression of *this with the Scalar type casted to NewScalar. |
||
743 |
template<typename NewScalar> |
||
744 |
174 |
JointModelRevoluteTpl<NewScalar,Options,axis> cast() const |
|
745 |
{ |
||
746 |
typedef JointModelRevoluteTpl<NewScalar,Options,axis> ReturnType; |
||
747 |
174 |
ReturnType res; |
|
748 |
174 |
res.setIndexes(id(),idx_q(),idx_v()); |
|
749 |
174 |
return res; |
|
750 |
} |
||
751 |
|||
752 |
}; // struct JointModelRevoluteTpl |
||
753 |
|||
754 |
typedef JointRevoluteTpl<double,0,0> JointRX; |
||
755 |
typedef JointDataRevoluteTpl<double,0,0> JointDataRX; |
||
756 |
typedef JointModelRevoluteTpl<double,0,0> JointModelRX; |
||
757 |
|||
758 |
typedef JointRevoluteTpl<double,0,1> JointRY; |
||
759 |
typedef JointDataRevoluteTpl<double,0,1> JointDataRY; |
||
760 |
typedef JointModelRevoluteTpl<double,0,1> JointModelRY; |
||
761 |
|||
762 |
typedef JointRevoluteTpl<double,0,2> JointRZ; |
||
763 |
typedef JointDataRevoluteTpl<double,0,2> JointDataRZ; |
||
764 |
typedef JointModelRevoluteTpl<double,0,2> JointModelRZ; |
||
765 |
|||
766 |
} //namespace pinocchio |
||
767 |
|||
768 |
#include <boost/type_traits.hpp> |
||
769 |
|||
770 |
namespace boost |
||
771 |
{ |
||
772 |
template<typename Scalar, int Options, int axis> |
||
773 |
struct has_nothrow_constructor< ::pinocchio::JointModelRevoluteTpl<Scalar,Options,axis> > |
||
774 |
: public integral_constant<bool,true> {}; |
||
775 |
|||
776 |
template<typename Scalar, int Options, int axis> |
||
777 |
struct has_nothrow_copy< ::pinocchio::JointModelRevoluteTpl<Scalar,Options,axis> > |
||
778 |
: public integral_constant<bool,true> {}; |
||
779 |
|||
780 |
template<typename Scalar, int Options, int axis> |
||
781 |
struct has_nothrow_constructor< ::pinocchio::JointDataRevoluteTpl<Scalar,Options,axis> > |
||
782 |
: public integral_constant<bool,true> {}; |
||
783 |
|||
784 |
template<typename Scalar, int Options, int axis> |
||
785 |
struct has_nothrow_copy< ::pinocchio::JointDataRevoluteTpl<Scalar,Options,axis> > |
||
786 |
: public integral_constant<bool,true> {}; |
||
787 |
} |
||
788 |
|||
789 |
#endif // ifndef __pinocchio_joint_revolute_hpp__ |
| Generated by: GCOVR (Version 4.2) |