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_prismatic_hpp__ |
||
7 |
#define __pinocchio_joint_prismatic_hpp__ |
||
8 |
|||
9 |
#include "pinocchio/macros.hpp" |
||
10 |
#include "pinocchio/multibody/joint/joint-base.hpp" |
||
11 |
#include "pinocchio/multibody/constraint.hpp" |
||
12 |
#include "pinocchio/spatial/inertia.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 MotionPrismaticTpl; |
||
20 |
|||
21 |
template<typename Scalar, int Options, int axis> |
||
22 |
struct SE3GroupAction< MotionPrismaticTpl<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< MotionPrismaticTpl<Scalar,Options,axis>, MotionDerived> |
||
29 |
{ |
||
30 |
typedef MotionTpl<Scalar,Options> ReturnType; |
||
31 |
}; |
||
32 |
|||
33 |
template<typename _Scalar, int _Options, int _axis> |
||
34 |
struct traits < MotionPrismaticTpl<_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 |
}; // struct traits MotionPrismaticTpl |
||
57 |
|||
58 |
template<typename _Scalar, int _Options, int _axis> |
||
59 |
struct MotionPrismaticTpl |
||
60 |
: MotionBase < MotionPrismaticTpl<_Scalar,_Options,_axis> > |
||
61 |
{ |
||
62 |
EIGEN_MAKE_ALIGNED_OPERATOR_NEW |
||
63 |
MOTION_TYPEDEF_TPL(MotionPrismaticTpl); |
||
64 |
|||
65 |
enum { axis = _axis }; |
||
66 |
|||
67 |
typedef SpatialAxis<_axis+LINEAR> Axis; |
||
68 |
typedef typename Axis::CartesianAxis3 CartesianAxis3; |
||
69 |
|||
70 |
42 |
MotionPrismaticTpl() {} |
|
71 |
18778 |
MotionPrismaticTpl(const Scalar & v) : m_v(v) {} |
|
72 |
|||
73 |
✓✗ | 12 |
inline PlainReturnType plain() const { return Axis() * m_v; } |
74 |
|||
75 |
template<typename OtherScalar> |
||
76 |
6 |
MotionPrismaticTpl __mult__(const OtherScalar & alpha) const |
|
77 |
{ |
||
78 |
6 |
return MotionPrismaticTpl(alpha*m_v); |
|
79 |
} |
||
80 |
|||
81 |
template<typename Derived> |
||
82 |
10 |
void addTo(MotionDense<Derived> & other) const |
|
83 |
{ |
||
84 |
typedef typename MotionDense<Derived>::Scalar OtherScalar; |
||
85 |
✓✗ | 10 |
other.linear()[_axis] += (OtherScalar) m_v; |
86 |
} |
||
87 |
|||
88 |
template<typename MotionDerived> |
||
89 |
6208 |
void setTo(MotionDense<MotionDerived> & other) const |
|
90 |
{ |
||
91 |
✓✓ | 24832 |
for(Eigen::DenseIndex k = 0; k < 3; ++k) |
92 |
✓✓✓✗ |
18624 |
other.linear()[k] = k == axis ? m_v : (Scalar)0; |
93 |
✓✗ | 6208 |
other.angular().setZero(); |
94 |
6208 |
} |
|
95 |
|||
96 |
template<typename S2, int O2, typename D2> |
||
97 |
6 |
void se3Action_impl(const SE3Tpl<S2,O2> & m, MotionDense<D2> & v) const |
|
98 |
{ |
||
99 |
✓✗ | 6 |
v.angular().setZero(); |
100 |
✓✗✓✗ ✓✗✓✗ |
6 |
v.linear().noalias() = m_v * (m.rotation().col(axis)); |
101 |
6 |
} |
|
102 |
|||
103 |
template<typename S2, int O2> |
||
104 |
6 |
MotionPlain se3Action_impl(const SE3Tpl<S2,O2> & m) const |
|
105 |
{ |
||
106 |
6 |
MotionPlain res; |
|
107 |
6 |
se3Action_impl(m,res); |
|
108 |
6 |
return res; |
|
109 |
} |
||
110 |
|||
111 |
template<typename S2, int O2, typename D2> |
||
112 |
22 |
void se3ActionInverse_impl(const SE3Tpl<S2,O2> & m, MotionDense<D2> & v) const |
|
113 |
{ |
||
114 |
// Linear |
||
115 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
22 |
v.linear().noalias() = m_v * (m.rotation().transpose().col(axis)); |
116 |
|||
117 |
// Angular |
||
118 |
✓✗ | 22 |
v.angular().setZero(); |
119 |
22 |
} |
|
120 |
|||
121 |
template<typename S2, int O2> |
||
122 |
22 |
MotionPlain se3ActionInverse_impl(const SE3Tpl<S2,O2> & m) const |
|
123 |
{ |
||
124 |
22 |
MotionPlain res; |
|
125 |
22 |
se3ActionInverse_impl(m,res); |
|
126 |
22 |
return res; |
|
127 |
} |
||
128 |
|||
129 |
template<typename M1, typename M2> |
||
130 |
18 |
void motionAction(const MotionDense<M1> & v, MotionDense<M2> & mout) const |
|
131 |
{ |
||
132 |
// Linear |
||
133 |
✓✗✓✗ |
18 |
CartesianAxis3::alphaCross(-m_v,v.angular(),mout.linear()); |
134 |
|||
135 |
// Angular |
||
136 |
✓✗ | 18 |
mout.angular().setZero(); |
137 |
18 |
} |
|
138 |
|||
139 |
template<typename M1> |
||
140 |
18 |
MotionPlain motionAction(const MotionDense<M1> & v) const |
|
141 |
{ |
||
142 |
18 |
MotionPlain res; |
|
143 |
18 |
motionAction(v,res); |
|
144 |
18 |
return res; |
|
145 |
} |
||
146 |
|||
147 |
6392 |
Scalar & linearRate() { return m_v; } |
|
148 |
const Scalar & linearRate() const { return m_v; } |
||
149 |
|||
150 |
102 |
bool isEqual_impl(const MotionPrismaticTpl & other) const |
|
151 |
{ |
||
152 |
102 |
return m_v == other.m_v; |
|
153 |
} |
||
154 |
|||
155 |
protected: |
||
156 |
|||
157 |
Scalar m_v; |
||
158 |
}; // struct MotionPrismaticTpl |
||
159 |
|||
160 |
template<typename Scalar, int Options, int axis, typename MotionDerived> |
||
161 |
typename MotionDerived::MotionPlain |
||
162 |
operator+(const MotionPrismaticTpl<Scalar,Options,axis> & m1, |
||
163 |
const MotionDense<MotionDerived> & m2) |
||
164 |
{ |
||
165 |
typename MotionDerived::MotionPlain res(m2); |
||
166 |
res += m1; |
||
167 |
return res; |
||
168 |
} |
||
169 |
|||
170 |
template<typename MotionDerived, typename S2, int O2, int axis> |
||
171 |
EIGEN_STRONG_INLINE |
||
172 |
typename MotionDerived::MotionPlain |
||
173 |
12 |
operator^(const MotionDense<MotionDerived> & m1, const MotionPrismaticTpl<S2,O2,axis> & m2) |
|
174 |
{ |
||
175 |
12 |
return m2.motionAction(m1); |
|
176 |
} |
||
177 |
|||
178 |
template<typename Scalar, int Options, int axis> struct TransformPrismaticTpl; |
||
179 |
|||
180 |
template<typename _Scalar, int _Options, int _axis> |
||
181 |
struct traits< TransformPrismaticTpl<_Scalar,_Options,_axis> > |
||
182 |
{ |
||
183 |
enum { |
||
184 |
axis = _axis, |
||
185 |
Options = _Options, |
||
186 |
LINEAR = 0, |
||
187 |
ANGULAR = 3 |
||
188 |
}; |
||
189 |
typedef _Scalar Scalar; |
||
190 |
typedef SE3Tpl<Scalar,Options> PlainType; |
||
191 |
typedef Eigen::Matrix<Scalar,3,1,Options> Vector3; |
||
192 |
typedef Eigen::Matrix<Scalar,3,3,Options> Matrix3; |
||
193 |
typedef typename Matrix3::IdentityReturnType AngularType; |
||
194 |
typedef AngularType AngularRef; |
||
195 |
typedef AngularType ConstAngularRef; |
||
196 |
typedef Vector3 LinearType; |
||
197 |
typedef const Vector3 LinearRef; |
||
198 |
typedef const Vector3 ConstLinearRef; |
||
199 |
typedef typename traits<PlainType>::ActionMatrixType ActionMatrixType; |
||
200 |
typedef typename traits<PlainType>::HomogeneousMatrixType HomogeneousMatrixType; |
||
201 |
}; // traits TransformPrismaticTpl |
||
202 |
|||
203 |
template<typename Scalar, int Options, int axis> |
||
204 |
struct SE3GroupAction< TransformPrismaticTpl<Scalar,Options,axis> > |
||
205 |
{ typedef typename traits <TransformPrismaticTpl<Scalar,Options,axis> >::PlainType ReturnType; }; |
||
206 |
|||
207 |
template<typename _Scalar, int _Options, int axis> |
||
208 |
struct TransformPrismaticTpl : SE3Base< TransformPrismaticTpl<_Scalar,_Options,axis> > |
||
209 |
{ |
||
210 |
EIGEN_MAKE_ALIGNED_OPERATOR_NEW |
||
211 |
PINOCCHIO_SE3_TYPEDEF_TPL(TransformPrismaticTpl); |
||
212 |
|||
213 |
typedef SpatialAxis<axis+LINEAR> Axis; |
||
214 |
typedef typename Axis::CartesianAxis3 CartesianAxis3; |
||
215 |
|||
216 |
42 |
TransformPrismaticTpl() {} |
|
217 |
18744 |
TransformPrismaticTpl(const Scalar & displacement) |
|
218 |
18774 |
: m_displacement(displacement) |
|
219 |
18774 |
{} |
|
220 |
|||
221 |
36920 |
PlainType plain() const |
|
222 |
{ |
||
223 |
36920 |
PlainType res(PlainType::Identity()); |
|
224 |
36920 |
res.rotation().setIdentity(); |
|
225 |
36920 |
res.translation()[axis] = m_displacement; |
|
226 |
|||
227 |
36920 |
return res; |
|
228 |
} |
||
229 |
|||
230 |
36920 |
operator PlainType() const { return plain(); } |
|
231 |
|||
232 |
template<typename S2, int O2> |
||
233 |
typename SE3GroupAction<TransformPrismaticTpl>::ReturnType |
||
234 |
se3action(const SE3Tpl<S2,O2> & m) const |
||
235 |
{ |
||
236 |
typedef typename SE3GroupAction<TransformPrismaticTpl>::ReturnType ReturnType; |
||
237 |
ReturnType res(m); |
||
238 |
res.translation()[axis] += m_displacement; |
||
239 |
|||
240 |
return res; |
||
241 |
} |
||
242 |
|||
243 |
2 |
const Scalar & displacement() const { return m_displacement; } |
|
244 |
37062 |
Scalar & displacement() { return m_displacement; } |
|
245 |
|||
246 |
✓✗ | 2 |
ConstLinearRef translation() const { return CartesianAxis3()*displacement(); }; |
247 |
✓✗ | 2 |
AngularType rotation() const { return AngularType(3,3); } |
248 |
|||
249 |
102 |
bool isEqual(const TransformPrismaticTpl & other) const |
|
250 |
{ |
||
251 |
102 |
return m_displacement == other.m_displacement; |
|
252 |
} |
||
253 |
|||
254 |
protected: |
||
255 |
|||
256 |
Scalar m_displacement; |
||
257 |
}; |
||
258 |
|||
259 |
template<typename Scalar, int Options, int axis> struct ConstraintPrismaticTpl; |
||
260 |
|||
261 |
template<typename _Scalar, int _Options, int axis> |
||
262 |
struct traits< ConstraintPrismaticTpl<_Scalar,_Options,axis> > |
||
263 |
{ |
||
264 |
typedef _Scalar Scalar; |
||
265 |
enum { Options = _Options }; |
||
266 |
enum { |
||
267 |
LINEAR = 0, |
||
268 |
ANGULAR = 3 |
||
269 |
}; |
||
270 |
typedef MotionPrismaticTpl<Scalar,Options,axis> JointMotion; |
||
271 |
typedef Eigen::Matrix<Scalar,1,1,Options> JointForce; |
||
272 |
typedef Eigen::Matrix<Scalar,6,1,Options> DenseBase; |
||
273 |
typedef DenseBase MatrixReturnType; |
||
274 |
typedef const DenseBase ConstMatrixReturnType; |
||
275 |
}; // traits ConstraintRevolute |
||
276 |
|||
277 |
template<typename Scalar, int Options, int axis> |
||
278 |
struct SE3GroupAction< ConstraintPrismaticTpl<Scalar,Options,axis> > |
||
279 |
{ typedef Eigen::Matrix<Scalar,6,1,Options> ReturnType; }; |
||
280 |
|||
281 |
template<typename Scalar, int Options, int axis, typename MotionDerived> |
||
282 |
struct MotionAlgebraAction< ConstraintPrismaticTpl<Scalar,Options,axis>, MotionDerived > |
||
283 |
{ typedef Eigen::Matrix<Scalar,6,1,Options> ReturnType; }; |
||
284 |
|||
285 |
template<typename Scalar, int Options, int axis, typename ForceDerived> |
||
286 |
struct ConstraintForceOp< ConstraintPrismaticTpl<Scalar,Options,axis>, ForceDerived> |
||
287 |
{ typedef typename ForceDense<ForceDerived>::ConstLinearType::template ConstFixedSegmentReturnType<1>::Type ReturnType; }; |
||
288 |
|||
289 |
template<typename Scalar, int Options, int axis, typename ForceSet> |
||
290 |
struct ConstraintForceSetOp< ConstraintPrismaticTpl<Scalar,Options,axis>, ForceSet> |
||
291 |
{ typedef typename Eigen::MatrixBase<ForceSet>::ConstRowXpr ReturnType; }; |
||
292 |
|||
293 |
template<typename _Scalar, int _Options, int axis> |
||
294 |
struct ConstraintPrismaticTpl |
||
295 |
: ConstraintBase < ConstraintPrismaticTpl <_Scalar,_Options,axis> > |
||
296 |
{ |
||
297 |
EIGEN_MAKE_ALIGNED_OPERATOR_NEW |
||
298 |
PINOCCHIO_CONSTRAINT_TYPEDEF_TPL(ConstraintPrismaticTpl) |
||
299 |
enum { NV = 1 }; |
||
300 |
|||
301 |
typedef SpatialAxis<LINEAR+axis> Axis; |
||
302 |
|||
303 |
18810 |
ConstraintPrismaticTpl() {}; |
|
304 |
|||
305 |
template<typename Vector1Like> |
||
306 |
28 |
JointMotion __mult__(const Eigen::MatrixBase<Vector1Like> & v) const |
|
307 |
{ |
||
308 |
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector1Like,1); |
||
309 |
✗✓ | 28 |
assert(v.size() == 1); |
310 |
28 |
return JointMotion(v[0]); |
|
311 |
} |
||
312 |
|||
313 |
template<typename S2, int O2> |
||
314 |
typename SE3GroupAction<ConstraintPrismaticTpl>::ReturnType |
||
315 |
34 |
se3Action(const SE3Tpl<S2,O2> & m) const |
|
316 |
{ |
||
317 |
✓✗ | 34 |
typename SE3GroupAction<ConstraintPrismaticTpl>::ReturnType res; |
318 |
✓✗ | 34 |
MotionRef<DenseBase> v(res); |
319 |
✓✗✓✗ ✓✗✓✗ |
34 |
v.linear() = m.rotation().col(axis); |
320 |
✓✗✓✗ |
34 |
v.angular().setZero(); |
321 |
68 |
return res; |
|
322 |
} |
||
323 |
|||
324 |
template<typename S2, int O2> |
||
325 |
typename SE3GroupAction<ConstraintPrismaticTpl>::ReturnType |
||
326 |
38 |
se3ActionInverse(const SE3Tpl<S2,O2> & m) const |
|
327 |
{ |
||
328 |
✓✗ | 38 |
typename SE3GroupAction<ConstraintPrismaticTpl>::ReturnType res; |
329 |
✓✗ | 38 |
MotionRef<DenseBase> v(res); |
330 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
38 |
v.linear() = m.rotation().transpose().col(axis); |
331 |
✓✗✓✗ |
38 |
v.angular().setZero(); |
332 |
76 |
return res; |
|
333 |
} |
||
334 |
|||
335 |
114 |
int nv_impl() const { return NV; } |
|
336 |
|||
337 |
struct TransposeConst |
||
338 |
{ |
||
339 |
const ConstraintPrismaticTpl & ref; |
||
340 |
56 |
TransposeConst(const ConstraintPrismaticTpl & ref) : ref(ref) {} |
|
341 |
|||
342 |
template<typename ForceDerived> |
||
343 |
typename ConstraintForceOp<ConstraintPrismaticTpl,ForceDerived>::ReturnType |
||
344 |
30 |
operator* (const ForceDense<ForceDerived> & f) const |
|
345 |
✓✗ | 30 |
{ return f.linear().template segment<1>(axis); } |
346 |
|||
347 |
/* [CRBA] MatrixBase operator* (Constraint::Transpose S, ForceSet::Block) */ |
||
348 |
template<typename Derived> |
||
349 |
typename ConstraintForceSetOp<ConstraintPrismaticTpl,Derived>::ReturnType |
||
350 |
26 |
operator*(const Eigen::MatrixBase<Derived> & F ) |
|
351 |
{ |
||
352 |
✗✓ | 26 |
assert(F.rows()==6); |
353 |
26 |
return F.row(LINEAR+axis); |
|
354 |
} |
||
355 |
|||
356 |
}; // struct TransposeConst |
||
357 |
56 |
TransposeConst transpose() const { return TransposeConst(*this); } |
|
358 |
|||
359 |
/* CRBA joint operators |
||
360 |
* - ForceSet::Block = ForceSet |
||
361 |
* - ForceSet operator* (Inertia Y,Constraint S) |
||
362 |
* - MatrixBase operator* (Constraint::Transpose S, ForceSet::Block) |
||
363 |
* - SE3::act(ForceSet::Block) |
||
364 |
*/ |
||
365 |
168 |
DenseBase matrix_impl() const |
|
366 |
{ |
||
367 |
✓✗ | 168 |
DenseBase S; |
368 |
✓✗ | 168 |
MotionRef<DenseBase> v(S); |
369 |
✓✗ | 168 |
v << Axis(); |
370 |
336 |
return S; |
|
371 |
} |
||
372 |
|||
373 |
template<typename MotionDerived> |
||
374 |
typename MotionAlgebraAction<ConstraintPrismaticTpl,MotionDerived>::ReturnType |
||
375 |
24 |
motionAction(const MotionDense<MotionDerived> & m) const |
|
376 |
{ |
||
377 |
✓✗ | 24 |
typename MotionAlgebraAction<ConstraintPrismaticTpl,MotionDerived>::ReturnType res; |
378 |
✓✗ | 24 |
MotionRef<DenseBase> v(res); |
379 |
✓✗✓✗ |
24 |
v = m.cross(Axis()); |
380 |
48 |
return res; |
|
381 |
} |
||
382 |
|||
383 |
102 |
bool isEqual(const ConstraintPrismaticTpl &) const { return true; } |
|
384 |
|||
385 |
}; // struct ConstraintPrismaticTpl |
||
386 |
|||
387 |
template<typename S1, int O1,typename S2, int O2, int axis> |
||
388 |
struct MultiplicationOp<InertiaTpl<S1,O1>, ConstraintPrismaticTpl<S2,O2,axis> > |
||
389 |
{ |
||
390 |
typedef Eigen::Matrix<S2,6,1,O2> ReturnType; |
||
391 |
}; |
||
392 |
|||
393 |
/* [CRBA] ForceSet operator* (Inertia Y,Constraint S) */ |
||
394 |
namespace impl |
||
395 |
{ |
||
396 |
template<typename S1, int O1, typename S2, int O2> |
||
397 |
struct LhsMultiplicationOp<InertiaTpl<S1,O1>, ConstraintPrismaticTpl<S2,O2,0> > |
||
398 |
{ |
||
399 |
typedef InertiaTpl<S1,O1> Inertia; |
||
400 |
typedef ConstraintPrismaticTpl<S2,O2,0> Constraint; |
||
401 |
typedef typename MultiplicationOp<Inertia,Constraint>::ReturnType ReturnType; |
||
402 |
8 |
static inline ReturnType run(const Inertia & Y, |
|
403 |
const Constraint & /*constraint*/) |
||
404 |
{ |
||
405 |
8 |
ReturnType res; |
|
406 |
|||
407 |
/* Y(:,0) = ( 1,0, 0, 0 , z , -y ) */ |
||
408 |
const S1 |
||
409 |
8 |
&m = Y.mass(), |
|
410 |
8 |
&y = Y.lever()[1], |
|
411 |
8 |
&z = Y.lever()[2]; |
|
412 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
8 |
res << m, S1(0), S1(0), S1(0), m*z, -m*y; |
413 |
|||
414 |
8 |
return res; |
|
415 |
} |
||
416 |
}; |
||
417 |
|||
418 |
template<typename S1, int O1, typename S2, int O2> |
||
419 |
struct LhsMultiplicationOp<InertiaTpl<S1,O1>, ConstraintPrismaticTpl<S2,O2,1> > |
||
420 |
{ |
||
421 |
typedef InertiaTpl<S1,O1> Inertia; |
||
422 |
typedef ConstraintPrismaticTpl<S2,O2,1> Constraint; |
||
423 |
typedef typename MultiplicationOp<Inertia,Constraint>::ReturnType ReturnType; |
||
424 |
4 |
static inline ReturnType run(const Inertia & Y, |
|
425 |
const Constraint & /*constraint*/) |
||
426 |
{ |
||
427 |
4 |
ReturnType res; |
|
428 |
|||
429 |
/* Y(:,1) = ( 0,1, 0, -z , 0 , x) */ |
||
430 |
const S1 |
||
431 |
4 |
&m = Y.mass(), |
|
432 |
4 |
&x = Y.lever()[0], |
|
433 |
4 |
&z = Y.lever()[2]; |
|
434 |
|||
435 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
4 |
res << S1(0), m, S1(0), -m*z, S1(0), m*x; |
436 |
|||
437 |
4 |
return res; |
|
438 |
} |
||
439 |
}; |
||
440 |
|||
441 |
template<typename S1, int O1, typename S2, int O2> |
||
442 |
struct LhsMultiplicationOp<InertiaTpl<S1,O1>, ConstraintPrismaticTpl<S2,O2,2> > |
||
443 |
{ |
||
444 |
typedef InertiaTpl<S1,O1> Inertia; |
||
445 |
typedef ConstraintPrismaticTpl<S2,O2,2> Constraint; |
||
446 |
typedef typename MultiplicationOp<Inertia,Constraint>::ReturnType ReturnType; |
||
447 |
4 |
static inline ReturnType run(const Inertia & Y, |
|
448 |
const Constraint & /*constraint*/) |
||
449 |
{ |
||
450 |
4 |
ReturnType res; |
|
451 |
|||
452 |
/* Y(:,2) = ( 0,0, 1, y , -x , 0) */ |
||
453 |
const S1 |
||
454 |
4 |
&m = Y.mass(), |
|
455 |
4 |
&x = Y.lever()[0], |
|
456 |
4 |
&y = Y.lever()[1]; |
|
457 |
|||
458 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
4 |
res << S1(0), S1(0), m, m*y, -m*x, S1(0); |
459 |
|||
460 |
4 |
return res; |
|
461 |
} |
||
462 |
}; |
||
463 |
} // namespace impl |
||
464 |
|||
465 |
template<typename M6Like,typename S2, int O2, int axis> |
||
466 |
struct MultiplicationOp<Eigen::MatrixBase<M6Like>, ConstraintPrismaticTpl<S2,O2,axis> > |
||
467 |
{ |
||
468 |
typedef typename M6Like::ConstColXpr ReturnType; |
||
469 |
}; |
||
470 |
|||
471 |
/* [ABA] operator* (Inertia Y,Constraint S) */ |
||
472 |
namespace impl |
||
473 |
{ |
||
474 |
template<typename M6Like, typename Scalar, int Options, int axis> |
||
475 |
struct LhsMultiplicationOp<Eigen::MatrixBase<M6Like>, ConstraintPrismaticTpl<Scalar,Options,axis> > |
||
476 |
{ |
||
477 |
typedef ConstraintPrismaticTpl<Scalar,Options,axis> Constraint; |
||
478 |
typedef typename MultiplicationOp<Eigen::MatrixBase<M6Like>,Constraint>::ReturnType ReturnType; |
||
479 |
12 |
static inline ReturnType run(const Eigen::MatrixBase<M6Like> & Y, |
|
480 |
const Constraint & /*constraint*/) |
||
481 |
{ |
||
482 |
EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(M6Like,6,6); |
||
483 |
12 |
return Y.derived().col(Inertia::LINEAR + axis); |
|
484 |
} |
||
485 |
}; |
||
486 |
} // namespace impl |
||
487 |
|||
488 |
template<typename _Scalar, int _Options, int _axis> |
||
489 |
struct JointPrismaticTpl |
||
490 |
{ |
||
491 |
typedef _Scalar Scalar; |
||
492 |
|||
493 |
enum |
||
494 |
{ |
||
495 |
Options = _Options, |
||
496 |
axis = _axis |
||
497 |
}; |
||
498 |
}; |
||
499 |
|||
500 |
template<typename _Scalar, int _Options, int axis> |
||
501 |
struct traits< JointPrismaticTpl<_Scalar,_Options,axis> > |
||
502 |
{ |
||
503 |
enum { |
||
504 |
NQ = 1, |
||
505 |
NV = 1 |
||
506 |
}; |
||
507 |
typedef _Scalar Scalar; |
||
508 |
enum { Options = _Options }; |
||
509 |
typedef JointDataPrismaticTpl<Scalar,Options,axis> JointDataDerived; |
||
510 |
typedef JointModelPrismaticTpl<Scalar,Options,axis> JointModelDerived; |
||
511 |
typedef ConstraintPrismaticTpl<Scalar,Options,axis> Constraint_t; |
||
512 |
typedef TransformPrismaticTpl<Scalar,Options,axis> Transformation_t; |
||
513 |
typedef MotionPrismaticTpl<Scalar,Options,axis> Motion_t; |
||
514 |
typedef MotionZeroTpl<Scalar,Options> Bias_t; |
||
515 |
|||
516 |
// [ABA] |
||
517 |
typedef Eigen::Matrix<Scalar,6,NV,Options> U_t; |
||
518 |
typedef Eigen::Matrix<Scalar,NV,NV,Options> D_t; |
||
519 |
typedef Eigen::Matrix<Scalar,6,NV,Options> UD_t; |
||
520 |
|||
521 |
PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE |
||
522 |
|||
523 |
typedef Eigen::Matrix<Scalar,NQ,1,Options> ConfigVector_t; |
||
524 |
typedef Eigen::Matrix<Scalar,NV,1,Options> TangentVector_t; |
||
525 |
}; |
||
526 |
|||
527 |
template<typename Scalar, int Options, int axis> |
||
528 |
struct traits< JointDataPrismaticTpl<Scalar,Options,axis> > |
||
529 |
{ typedef JointPrismaticTpl<Scalar,Options,axis> JointDerived; }; |
||
530 |
|||
531 |
template<typename Scalar, int Options, int axis> |
||
532 |
struct traits< JointModelPrismaticTpl<Scalar,Options,axis> > |
||
533 |
{ typedef JointPrismaticTpl<Scalar,Options,axis> JointDerived; }; |
||
534 |
|||
535 |
template<typename _Scalar, int _Options, int axis> |
||
536 |
struct JointDataPrismaticTpl : public JointDataBase< JointDataPrismaticTpl<_Scalar,_Options,axis> > |
||
537 |
{ |
||
538 |
180 |
EIGEN_MAKE_ALIGNED_OPERATOR_NEW |
|
539 |
typedef JointPrismaticTpl<_Scalar,_Options,axis> JointDerived; |
||
540 |
PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(JointDerived); |
||
541 |
2036 |
PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR |
|
542 |
|||
543 |
Constraint_t S; |
||
544 |
Transformation_t M; |
||
545 |
Motion_t v; |
||
546 |
Bias_t c; |
||
547 |
|||
548 |
// [ABA] specific data |
||
549 |
U_t U; |
||
550 |
D_t Dinv; |
||
551 |
UD_t UDinv; |
||
552 |
|||
553 |
18768 |
JointDataPrismaticTpl() |
|
554 |
: M((Scalar)0) |
||
555 |
, v((Scalar)0) |
||
556 |
, U(U_t::Zero()) |
||
557 |
, Dinv(D_t::Zero()) |
||
558 |
✓✗✓✗ ✓✗ |
18768 |
, UDinv(UD_t::Zero()) |
559 |
18768 |
{} |
|
560 |
|||
561 |
264 |
static std::string classname() |
|
562 |
{ |
||
563 |
✓✗✓✗ |
264 |
return std::string("JointDataP") + axisLabel<axis>(); |
564 |
} |
||
565 |
9 |
std::string shortname() const { return classname(); } |
|
566 |
|||
567 |
}; // struct JointDataPrismaticTpl |
||
568 |
|||
569 |
template<typename NewScalar, typename Scalar, int Options, int axis> |
||
570 |
struct CastType< NewScalar, JointModelPrismaticTpl<Scalar,Options,axis> > |
||
571 |
{ |
||
572 |
typedef JointModelPrismaticTpl<NewScalar,Options,axis> type; |
||
573 |
}; |
||
574 |
|||
575 |
template<typename _Scalar, int _Options, int axis> |
||
576 |
struct JointModelPrismaticTpl |
||
577 |
: public JointModelBase< JointModelPrismaticTpl<_Scalar,_Options,axis> > |
||
578 |
{ |
||
579 |
308 |
EIGEN_MAKE_ALIGNED_OPERATOR_NEW |
|
580 |
typedef JointPrismaticTpl<_Scalar,_Options,axis> JointDerived; |
||
581 |
PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived); |
||
582 |
|||
583 |
typedef JointModelBase<JointModelPrismaticTpl> Base; |
||
584 |
using Base::id; |
||
585 |
using Base::idx_q; |
||
586 |
using Base::idx_v; |
||
587 |
using Base::setIndexes; |
||
588 |
|||
589 |
18610 |
JointDataDerived createData() const { return JointDataDerived(); } |
|
590 |
|||
591 |
2 |
const std::vector<bool> hasConfigurationLimit() const |
|
592 |
{ |
||
593 |
✓✗ | 2 |
return {true}; |
594 |
} |
||
595 |
|||
596 |
2 |
const std::vector<bool> hasConfigurationLimitInTangent() const |
|
597 |
{ |
||
598 |
✓✗ | 2 |
return {true}; |
599 |
} |
||
600 |
|||
601 |
template<typename ConfigVector> |
||
602 |
36894 |
void calc(JointDataDerived & data, |
|
603 |
const typename Eigen::MatrixBase<ConfigVector> & qs) const |
||
604 |
{ |
||
605 |
typedef typename ConfigVector::Scalar Scalar; |
||
606 |
36894 |
const Scalar & q = qs[idx_q()]; |
|
607 |
36894 |
data.M.displacement() = q; |
|
608 |
36894 |
} |
|
609 |
|||
610 |
template<typename ConfigVector, typename TangentVector> |
||
611 |
6224 |
void calc(JointDataDerived & data, |
|
612 |
const typename Eigen::MatrixBase<ConfigVector> & qs, |
||
613 |
const typename Eigen::MatrixBase<TangentVector> & vs) const |
||
614 |
{ |
||
615 |
6224 |
calc(data,qs.derived()); |
|
616 |
|||
617 |
typedef typename TangentVector::Scalar S2; |
||
618 |
6224 |
const S2 & v = vs[idx_v()]; |
|
619 |
6224 |
data.v.linearRate() = v; |
|
620 |
6224 |
} |
|
621 |
|||
622 |
template<typename Matrix6Like> |
||
623 |
44 |
void calc_aba(JointDataDerived & data, const Eigen::MatrixBase<Matrix6Like> & I, const bool update_I) const |
|
624 |
{ |
||
625 |
✓✗ | 44 |
data.U = I.col(Inertia::LINEAR + axis); |
626 |
44 |
data.Dinv[0] = Scalar(1)/I(Inertia::LINEAR + axis, Inertia::LINEAR + axis); |
|
627 |
✓✗✓✗ |
44 |
data.UDinv.noalias() = data.U * data.Dinv[0]; |
628 |
|||
629 |
✓✓ | 44 |
if (update_I) |
630 |
✓✗✓✗ |
12 |
PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like,I) -= data.UDinv * data.U.transpose(); |
631 |
44 |
} |
|
632 |
|||
633 |
110571 |
static std::string classname() |
|
634 |
{ |
||
635 |
✓✗✓✗ |
110571 |
return std::string("JointModelP") + axisLabel<axis>(); |
636 |
} |
||
637 |
55161 |
std::string shortname() const { return classname(); } |
|
638 |
|||
639 |
/// \returns An expression of *this with the Scalar type casted to NewScalar. |
||
640 |
template<typename NewScalar> |
||
641 |
12 |
JointModelPrismaticTpl<NewScalar,Options,axis> cast() const |
|
642 |
{ |
||
643 |
typedef JointModelPrismaticTpl<NewScalar,Options,axis> ReturnType; |
||
644 |
12 |
ReturnType res; |
|
645 |
12 |
res.setIndexes(id(),idx_q(),idx_v()); |
|
646 |
12 |
return res; |
|
647 |
} |
||
648 |
|||
649 |
}; // struct JointModelPrismaticTpl |
||
650 |
|||
651 |
typedef JointPrismaticTpl<double,0,0> JointPX; |
||
652 |
typedef JointDataPrismaticTpl<double,0,0> JointDataPX; |
||
653 |
typedef JointModelPrismaticTpl<double,0,0> JointModelPX; |
||
654 |
|||
655 |
typedef JointPrismaticTpl<double,0,1> JointPY; |
||
656 |
typedef JointDataPrismaticTpl<double,0,1> JointDataPY; |
||
657 |
typedef JointModelPrismaticTpl<double,0,1> JointModelPY; |
||
658 |
|||
659 |
typedef JointPrismaticTpl<double,0,2> JointPZ; |
||
660 |
typedef JointDataPrismaticTpl<double,0,2> JointDataPZ; |
||
661 |
typedef JointModelPrismaticTpl<double,0,2> JointModelPZ; |
||
662 |
|||
663 |
} //namespace pinocchio |
||
664 |
|||
665 |
#include <boost/type_traits.hpp> |
||
666 |
|||
667 |
namespace boost |
||
668 |
{ |
||
669 |
template<typename Scalar, int Options, int axis> |
||
670 |
struct has_nothrow_constructor< ::pinocchio::JointModelPrismaticTpl<Scalar,Options,axis> > |
||
671 |
: public integral_constant<bool,true> {}; |
||
672 |
|||
673 |
template<typename Scalar, int Options, int axis> |
||
674 |
struct has_nothrow_copy< ::pinocchio::JointModelPrismaticTpl<Scalar,Options,axis> > |
||
675 |
: public integral_constant<bool,true> {}; |
||
676 |
|||
677 |
template<typename Scalar, int Options, int axis> |
||
678 |
struct has_nothrow_constructor< ::pinocchio::JointDataPrismaticTpl<Scalar,Options,axis> > |
||
679 |
: public integral_constant<bool,true> {}; |
||
680 |
|||
681 |
template<typename Scalar, int Options, int axis> |
||
682 |
struct has_nothrow_copy< ::pinocchio::JointDataPrismaticTpl<Scalar,Options,axis> > |
||
683 |
: public integral_constant<bool,true> {}; |
||
684 |
} |
||
685 |
|||
686 |
#endif // ifndef __pinocchio_joint_prismatic_hpp__ |
Generated by: GCOVR (Version 4.2) |