| 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_unaligned_hpp__ |
||
7 |
#define __pinocchio_joint_revolute_unaligned_hpp__ |
||
8 |
|||
9 |
#include "pinocchio/fwd.hpp" |
||
10 |
#include "pinocchio/multibody/joint/joint-base.hpp" |
||
11 |
#include "pinocchio/multibody/constraint.hpp" |
||
12 |
#include "pinocchio/spatial/inertia.hpp" |
||
13 |
#include "pinocchio/math/matrix.hpp" |
||
14 |
|||
15 |
namespace pinocchio |
||
16 |
{ |
||
17 |
|||
18 |
template<typename Scalar, int Options=0> struct MotionRevoluteUnalignedTpl; |
||
19 |
typedef MotionRevoluteUnalignedTpl<double> MotionRevoluteUnaligned; |
||
20 |
|||
21 |
template<typename Scalar, int Options> |
||
22 |
struct SE3GroupAction< MotionRevoluteUnalignedTpl<Scalar,Options> > |
||
23 |
{ |
||
24 |
typedef MotionTpl<Scalar,Options> ReturnType; |
||
25 |
}; |
||
26 |
|||
27 |
template<typename Scalar, int Options, typename MotionDerived> |
||
28 |
struct MotionAlgebraAction< MotionRevoluteUnalignedTpl<Scalar,Options>, MotionDerived> |
||
29 |
{ |
||
30 |
typedef MotionTpl<Scalar,Options> ReturnType; |
||
31 |
}; |
||
32 |
|||
33 |
template<typename _Scalar, int _Options> |
||
34 |
struct traits< MotionRevoluteUnalignedTpl<_Scalar,_Options> > |
||
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 MotionRevoluteUnalignedTpl |
||
57 |
|||
58 |
template<typename _Scalar, int _Options> |
||
59 |
struct MotionRevoluteUnalignedTpl |
||
60 |
: MotionBase< MotionRevoluteUnalignedTpl<_Scalar,_Options> > |
||
61 |
{ |
||
62 |
EIGEN_MAKE_ALIGNED_OPERATOR_NEW |
||
63 |
MOTION_TYPEDEF_TPL(MotionRevoluteUnalignedTpl); |
||
64 |
|||
65 |
14 |
MotionRevoluteUnalignedTpl() {} |
|
66 |
|||
67 |
template<typename Vector3Like, typename OtherScalar> |
||
68 |
229 |
MotionRevoluteUnalignedTpl(const Eigen::MatrixBase<Vector3Like> & axis, |
|
69 |
const OtherScalar & w) |
||
70 |
: m_axis(axis) |
||
71 |
229 |
, m_w(w) |
|
72 |
229 |
{} |
|
73 |
|||
74 |
3 |
inline PlainReturnType plain() const |
|
75 |
{ |
||
76 |
return PlainReturnType(PlainReturnType::Vector3::Zero(), |
||
77 |
✓✗✓✗ |
3 |
m_axis*m_w); |
78 |
} |
||
79 |
|||
80 |
template<typename OtherScalar> |
||
81 |
1 |
MotionRevoluteUnalignedTpl __mult__(const OtherScalar & alpha) const |
|
82 |
{ |
||
83 |
✓✗ | 2 |
return MotionRevoluteUnalignedTpl(m_axis,alpha*m_w); |
84 |
} |
||
85 |
|||
86 |
template<typename MotionDerived> |
||
87 |
4 |
inline void addTo(MotionDense<MotionDerived> & v) const |
|
88 |
{ |
||
89 |
✓✗✓✗ |
4 |
v.angular() += m_axis*m_w; |
90 |
4 |
} |
|
91 |
|||
92 |
template<typename Derived> |
||
93 |
43 |
void setTo(MotionDense<Derived> & other) const |
|
94 |
{ |
||
95 |
✓✗ | 43 |
other.linear().setZero(); |
96 |
✓✗✓✗ ✓✗ |
43 |
other.angular().noalias() = m_axis*m_w; |
97 |
43 |
} |
|
98 |
|||
99 |
template<typename S2, int O2, typename D2> |
||
100 |
1 |
void se3Action_impl(const SE3Tpl<S2,O2> & m, MotionDense<D2> & v) const |
|
101 |
{ |
||
102 |
// Angular |
||
103 |
✓✗✓✗ ✓✗✓✗ |
1 |
v.angular().noalias() = m_w * m.rotation() * m_axis; |
104 |
|||
105 |
// Linear |
||
106 |
✓✗✓✗ ✓✗✓✗ |
1 |
v.linear().noalias() = m.translation().cross(v.angular()); |
107 |
1 |
} |
|
108 |
|||
109 |
template<typename S2, int O2> |
||
110 |
1 |
MotionPlain se3Action_impl(const SE3Tpl<S2,O2> & m) const |
|
111 |
{ |
||
112 |
1 |
MotionPlain res; |
|
113 |
1 |
se3Action_impl(m,res); |
|
114 |
1 |
return res; |
|
115 |
} |
||
116 |
|||
117 |
template<typename S2, int O2, typename D2> |
||
118 |
1 |
void se3ActionInverse_impl(const SE3Tpl<S2,O2> & m, MotionDense<D2> & v) const |
|
119 |
{ |
||
120 |
// Linear |
||
121 |
// TODO: use v.angular() as temporary variable |
||
122 |
✓✗ | 1 |
Vector3 v3_tmp; |
123 |
✓✗✓✗ ✓✗✓✗ |
1 |
v3_tmp.noalias() = m_axis.cross(m.translation()); |
124 |
✓✗ | 1 |
v3_tmp *= m_w; |
125 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
1 |
v.linear().noalias() = m.rotation().transpose() * v3_tmp; |
126 |
|||
127 |
// Angular |
||
128 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
1 |
v.angular().noalias() = m.rotation().transpose() * m_axis; |
129 |
✓✗✓✗ |
1 |
v.angular() *= m_w; |
130 |
1 |
} |
|
131 |
|||
132 |
template<typename S2, int O2> |
||
133 |
1 |
MotionPlain se3ActionInverse_impl(const SE3Tpl<S2,O2> & m) const |
|
134 |
{ |
||
135 |
1 |
MotionPlain res; |
|
136 |
1 |
se3ActionInverse_impl(m,res); |
|
137 |
1 |
return res; |
|
138 |
} |
||
139 |
|||
140 |
template<typename M1, typename M2> |
||
141 |
7 |
void motionAction(const MotionDense<M1> & v, MotionDense<M2> & mout) const |
|
142 |
{ |
||
143 |
// Linear |
||
144 |
✓✗✓✗ ✓✗✓✗ |
7 |
mout.linear().noalias() = v.linear().cross(m_axis); |
145 |
✓✗ | 7 |
mout.linear() *= m_w; |
146 |
|||
147 |
// Angular |
||
148 |
✓✗✓✗ ✓✗✓✗ |
7 |
mout.angular().noalias() = v.angular().cross(m_axis); |
149 |
✓✗ | 7 |
mout.angular() *= m_w; |
150 |
7 |
} |
|
151 |
|||
152 |
template<typename M1> |
||
153 |
7 |
MotionPlain motionAction(const MotionDense<M1> & v) const |
|
154 |
{ |
||
155 |
7 |
MotionPlain res; |
|
156 |
7 |
motionAction(v,res); |
|
157 |
7 |
return res; |
|
158 |
} |
||
159 |
|||
160 |
34 |
bool isEqual_impl(const MotionRevoluteUnalignedTpl & other) const |
|
161 |
{ |
||
162 |
✓✗✓✗ |
34 |
return m_axis == other.m_axis && m_w == other.m_w; |
163 |
} |
||
164 |
|||
165 |
const Scalar & angularRate() const { return m_w; } |
||
166 |
101 |
Scalar & angularRate() { return m_w; } |
|
167 |
|||
168 |
const Vector3 & axis() const { return m_axis; } |
||
169 |
56 |
Vector3 & axis() { return m_axis; } |
|
170 |
|||
171 |
protected: |
||
172 |
Vector3 m_axis; |
||
173 |
Scalar m_w; |
||
174 |
|||
175 |
}; // struct MotionRevoluteUnalignedTpl |
||
176 |
|||
177 |
template<typename S1, int O1, typename MotionDerived> |
||
178 |
inline typename MotionDerived::MotionPlain |
||
179 |
operator+(const MotionRevoluteUnalignedTpl<S1,O1> & m1, |
||
180 |
const MotionDense<MotionDerived> & m2) |
||
181 |
{ |
||
182 |
typename MotionDerived::MotionPlain res(m2); |
||
183 |
res += m1; |
||
184 |
return res; |
||
185 |
} |
||
186 |
|||
187 |
template<typename MotionDerived, typename S2, int O2> |
||
188 |
inline typename MotionDerived::MotionPlain |
||
189 |
6 |
operator^(const MotionDense<MotionDerived> & m1, |
|
190 |
const MotionRevoluteUnalignedTpl<S2,O2> & m2) |
||
191 |
{ |
||
192 |
6 |
return m2.motionAction(m1); |
|
193 |
} |
||
194 |
|||
195 |
template<typename Scalar, int Options> struct ConstraintRevoluteUnalignedTpl; |
||
196 |
|||
197 |
template<typename _Scalar, int _Options> |
||
198 |
struct traits< ConstraintRevoluteUnalignedTpl<_Scalar,_Options> > |
||
199 |
{ |
||
200 |
typedef _Scalar Scalar; |
||
201 |
enum { Options = _Options }; |
||
202 |
enum { |
||
203 |
LINEAR = 0, |
||
204 |
ANGULAR = 3 |
||
205 |
}; |
||
206 |
typedef MotionRevoluteUnalignedTpl<Scalar,Options> JointMotion; |
||
207 |
typedef Eigen::Matrix<Scalar,1,1,Options> JointForce; |
||
208 |
typedef Eigen::Matrix<Scalar,6,1,Options> DenseBase; |
||
209 |
typedef DenseBase MatrixReturnType; |
||
210 |
typedef const DenseBase ConstMatrixReturnType; |
||
211 |
|||
212 |
typedef Eigen::Matrix<Scalar,3,1,Options> Vector3; |
||
213 |
}; // traits ConstraintRevoluteUnalignedTpl |
||
214 |
|||
215 |
template<typename Scalar, int Options> |
||
216 |
struct SE3GroupAction< ConstraintRevoluteUnalignedTpl<Scalar,Options> > |
||
217 |
{ typedef Eigen::Matrix<Scalar,6,1,Options> ReturnType; }; |
||
218 |
|||
219 |
template<typename Scalar, int Options, typename MotionDerived> |
||
220 |
struct MotionAlgebraAction< ConstraintRevoluteUnalignedTpl<Scalar,Options>, MotionDerived > |
||
221 |
{ typedef Eigen::Matrix<Scalar,6,1,Options> ReturnType; }; |
||
222 |
|||
223 |
template<typename Scalar, int Options, typename ForceDerived> |
||
224 |
struct ConstraintForceOp< ConstraintRevoluteUnalignedTpl<Scalar,Options>, ForceDerived> |
||
225 |
{ |
||
226 |
typedef typename traits< ConstraintRevoluteUnalignedTpl<Scalar,Options> >::Vector3 Vector3; |
||
227 |
typedef Eigen::Matrix<typename PINOCCHIO_EIGEN_DOT_PRODUCT_RETURN_TYPE(Vector3,typename ForceDense<ForceDerived>::ConstAngularType),1,1,Options> ReturnType; |
||
228 |
}; |
||
229 |
|||
230 |
template<typename Scalar, int Options, typename ForceSet> |
||
231 |
struct ConstraintForceSetOp< ConstraintRevoluteUnalignedTpl<Scalar,Options>, ForceSet> |
||
232 |
{ |
||
233 |
typedef typename traits< ConstraintRevoluteUnalignedTpl<Scalar,Options> >::Vector3 Vector3; |
||
234 |
typedef typename MatrixMatrixProduct<Eigen::Transpose<const Vector3>, |
||
235 |
typename Eigen::MatrixBase<const ForceSet>::template NRowsBlockXpr<3>::Type |
||
236 |
>::type ReturnType; |
||
237 |
}; |
||
238 |
|||
239 |
template<typename _Scalar, int _Options> |
||
240 |
struct ConstraintRevoluteUnalignedTpl |
||
241 |
: ConstraintBase< ConstraintRevoluteUnalignedTpl<_Scalar,_Options> > |
||
242 |
{ |
||
243 |
EIGEN_MAKE_ALIGNED_OPERATOR_NEW |
||
244 |
PINOCCHIO_CONSTRAINT_TYPEDEF_TPL(ConstraintRevoluteUnalignedTpl) |
||
245 |
|||
246 |
enum { NV = 1 }; |
||
247 |
|||
248 |
typedef typename traits<ConstraintRevoluteUnalignedTpl>::Vector3 Vector3; |
||
249 |
|||
250 |
14 |
ConstraintRevoluteUnalignedTpl() {} |
|
251 |
|||
252 |
template<typename Vector3Like> |
||
253 |
219 |
ConstraintRevoluteUnalignedTpl(const Eigen::MatrixBase<Vector3Like> & axis) |
|
254 |
219 |
: m_axis(axis) |
|
255 |
219 |
{} |
|
256 |
|||
257 |
template<typename Vector1Like> |
||
258 |
12 |
JointMotion __mult__(const Eigen::MatrixBase<Vector1Like> & v) const |
|
259 |
{ |
||
260 |
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector1Like,1); |
||
261 |
✓✗ | 16 |
return JointMotion(m_axis,v[0]); |
262 |
} |
||
263 |
|||
264 |
template<typename S1, int O1> |
||
265 |
typename SE3GroupAction<ConstraintRevoluteUnalignedTpl>::ReturnType |
||
266 |
12 |
se3Action(const SE3Tpl<S1,O1> & m) const |
|
267 |
{ |
||
268 |
typedef typename SE3GroupAction<ConstraintRevoluteUnalignedTpl>::ReturnType ReturnType; |
||
269 |
|||
270 |
/* X*S = [ R pxR ; 0 R ] [ 0 ; a ] = [ px(Ra) ; Ra ] */ |
||
271 |
12 |
ReturnType res; |
|
272 |
✓✗✓✗ ✓✗ |
12 |
res.template segment<3>(ANGULAR).noalias() = m.rotation() * m_axis; |
273 |
✓✗✓✗ ✓✗✓✗ |
12 |
res.template segment<3>(LINEAR).noalias() = m.translation().cross(res.template segment<3>(ANGULAR)); |
274 |
12 |
return res; |
|
275 |
} |
||
276 |
|||
277 |
template<typename S1, int O1> |
||
278 |
typename SE3GroupAction<ConstraintRevoluteUnalignedTpl>::ReturnType |
||
279 |
6 |
se3ActionInverse(const SE3Tpl<S1,O1> & m) const |
|
280 |
{ |
||
281 |
typedef typename SE3GroupAction<ConstraintRevoluteUnalignedTpl>::ReturnType ReturnType; |
||
282 |
|||
283 |
6 |
ReturnType res; |
|
284 |
✓✗✓✗ ✓✗✓✗ |
6 |
res.template segment<3>(ANGULAR).noalias() = m.rotation().transpose() * m_axis; |
285 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
6 |
res.template segment<3>(LINEAR).noalias() = -m.rotation().transpose() * m.translation().cross(m_axis); |
286 |
6 |
return res; |
|
287 |
} |
||
288 |
|||
289 |
35 |
int nv_impl() const { return NV; } |
|
290 |
|||
291 |
struct TransposeConst |
||
292 |
{ |
||
293 |
const ConstraintRevoluteUnalignedTpl & ref; |
||
294 |
16 |
TransposeConst(const ConstraintRevoluteUnalignedTpl & ref) : ref(ref) {} |
|
295 |
|||
296 |
template<typename ForceDerived> |
||
297 |
typename ConstraintForceOp<ConstraintRevoluteUnalignedTpl,ForceDerived>::ReturnType |
||
298 |
10 |
operator*(const ForceDense<ForceDerived> & f) const |
|
299 |
{ |
||
300 |
typedef typename ConstraintForceOp<ConstraintRevoluteUnalignedTpl,ForceDerived>::ReturnType ReturnType; |
||
301 |
10 |
ReturnType res; |
|
302 |
✓✗✓✗ |
10 |
res[0] = ref.axis().dot(f.angular()); |
303 |
10 |
return res; |
|
304 |
} |
||
305 |
|||
306 |
/* [CRBA] MatrixBase operator* (Constraint::Transpose S, ForceSet::Block) */ |
||
307 |
template<typename ForceSet> |
||
308 |
typename ConstraintForceSetOp<ConstraintRevoluteUnalignedTpl,ForceSet>::ReturnType |
||
309 |
6 |
operator*(const Eigen::MatrixBase<ForceSet> & F) |
|
310 |
{ |
||
311 |
EIGEN_STATIC_ASSERT(ForceSet::RowsAtCompileTime==6,THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE) |
||
312 |
/* Return ax.T * F[3:end,:] */ |
||
313 |
✓✗✓✗ |
12 |
return ref.axis().transpose() * F.template middleRows<3>(ANGULAR); |
314 |
} |
||
315 |
|||
316 |
}; |
||
317 |
|||
318 |
16 |
TransposeConst transpose() const { return TransposeConst(*this); } |
|
319 |
|||
320 |
/* CRBA joint operators |
||
321 |
* - ForceSet::Block = ForceSet |
||
322 |
* - ForceSet operator* (Inertia Y,Constraint S) |
||
323 |
* - MatrixBase operator* (Constraint::Transpose S, ForceSet::Block) |
||
324 |
* - SE3::act(ForceSet::Block) |
||
325 |
*/ |
||
326 |
42 |
DenseBase matrix_impl() const |
|
327 |
{ |
||
328 |
42 |
DenseBase S; |
|
329 |
✓✗ | 42 |
S.template segment<3>(LINEAR).setZero(); |
330 |
✓✗ | 42 |
S.template segment<3>(ANGULAR) = m_axis; |
331 |
42 |
return S; |
|
332 |
} |
||
333 |
|||
334 |
template<typename MotionDerived> |
||
335 |
typename MotionAlgebraAction<ConstraintRevoluteUnalignedTpl,MotionDerived>::ReturnType |
||
336 |
6 |
motionAction(const MotionDense<MotionDerived> & m) const |
|
337 |
{ |
||
338 |
✓✗ | 6 |
const typename MotionDerived::ConstLinearType v = m.linear(); |
339 |
✓✗ | 6 |
const typename MotionDerived::ConstAngularType w = m.angular(); |
340 |
|||
341 |
✓✗ | 6 |
DenseBase res; |
342 |
✓✗✓✗ ✓✗✓✗ |
6 |
res.template segment<3>(LINEAR).noalias() = v.cross(m_axis); |
343 |
✓✗✓✗ ✓✗✓✗ |
6 |
res.template segment<3>(ANGULAR).noalias() = w.cross(m_axis); |
344 |
|||
345 |
12 |
return res; |
|
346 |
} |
||
347 |
|||
348 |
36 |
const Vector3 & axis() const { return m_axis; } |
|
349 |
56 |
Vector3 & axis() { return m_axis; } |
|
350 |
|||
351 |
34 |
bool isEqual(const ConstraintRevoluteUnalignedTpl & other) const |
|
352 |
{ |
||
353 |
34 |
return m_axis == other.m_axis; |
|
354 |
} |
||
355 |
|||
356 |
protected: |
||
357 |
|||
358 |
Vector3 m_axis; |
||
359 |
|||
360 |
}; // struct ConstraintRevoluteUnalignedTpl |
||
361 |
|||
362 |
template<typename S1, int O1,typename S2, int O2> |
||
363 |
struct MultiplicationOp<InertiaTpl<S1,O1>, ConstraintRevoluteUnalignedTpl<S2,O2> > |
||
364 |
{ |
||
365 |
typedef Eigen::Matrix<S2,6,1,O2> ReturnType; |
||
366 |
}; |
||
367 |
|||
368 |
/* [CRBA] ForceSet operator* (Inertia Y,Constraint S) */ |
||
369 |
namespace impl |
||
370 |
{ |
||
371 |
template<typename S1, int O1, typename S2, int O2> |
||
372 |
struct LhsMultiplicationOp<InertiaTpl<S1,O1>, ConstraintRevoluteUnalignedTpl<S2,O2> > |
||
373 |
{ |
||
374 |
typedef InertiaTpl<S1,O1> Inertia; |
||
375 |
typedef ConstraintRevoluteUnalignedTpl<S2,O2> Constraint; |
||
376 |
typedef typename MultiplicationOp<Inertia,Constraint>::ReturnType ReturnType; |
||
377 |
8 |
static inline ReturnType run(const Inertia & Y, |
|
378 |
const Constraint & cru) |
||
379 |
{ |
||
380 |
8 |
ReturnType res; |
|
381 |
|||
382 |
/* YS = [ m -mcx ; mcx I-mcxcx ] [ 0 ; w ] = [ mcxw ; Iw -mcxcxw ] */ |
||
383 |
8 |
const typename Inertia::Scalar & m = Y.mass(); |
|
384 |
8 |
const typename Inertia::Vector3 & c = Y.lever(); |
|
385 |
8 |
const typename Inertia::Symmetric3 & I = Y.inertia(); |
|
386 |
|||
387 |
✓✗✓✗ ✓✗ |
8 |
res.template segment<3>(Inertia::LINEAR) = -m*c.cross(cru.axis()); |
388 |
✓✗✓✗ ✓✗ |
8 |
res.template segment<3>(Inertia::ANGULAR).noalias() = I*cru.axis(); |
389 |
✓✗✓✗ ✓✗ |
8 |
res.template segment<3>(Inertia::ANGULAR) += c.cross(res.template segment<3>(Inertia::LINEAR)); |
390 |
|||
391 |
8 |
return res; |
|
392 |
} |
||
393 |
}; |
||
394 |
} // namespace impl |
||
395 |
|||
396 |
template<typename M6Like, typename Scalar, int Options> |
||
397 |
struct MultiplicationOp<Eigen::MatrixBase<M6Like>, ConstraintRevoluteUnalignedTpl<Scalar,Options> > |
||
398 |
{ |
||
399 |
typedef typename SizeDepType<3>::ColsReturn<M6Like>::ConstType M6LikeCols; |
||
400 |
typedef typename Eigen::internal::remove_const<M6LikeCols>::type M6LikeColsNonConst; |
||
401 |
|||
402 |
typedef ConstraintRevoluteUnalignedTpl<Scalar,Options> Constraint; |
||
403 |
typedef typename Constraint::Vector3 Vector3; |
||
404 |
typedef const typename MatrixMatrixProduct<M6LikeColsNonConst,Vector3>::type ReturnType; |
||
405 |
}; |
||
406 |
|||
407 |
/* [ABA] operator* (Inertia Y,Constraint S) */ |
||
408 |
namespace impl |
||
409 |
{ |
||
410 |
template<typename M6Like, typename Scalar, int Options> |
||
411 |
struct LhsMultiplicationOp<Eigen::MatrixBase<M6Like>, ConstraintRevoluteUnalignedTpl<Scalar,Options> > |
||
412 |
{ |
||
413 |
typedef ConstraintRevoluteUnalignedTpl<Scalar,Options> Constraint; |
||
414 |
typedef typename MultiplicationOp<Eigen::MatrixBase<M6Like>,Constraint>::ReturnType ReturnType; |
||
415 |
|||
416 |
4 |
static inline ReturnType run(const Eigen::MatrixBase<M6Like> & Y, |
|
417 |
const Constraint & cru) |
||
418 |
{ |
||
419 |
EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(M6Like,6,6); |
||
420 |
✓✗ | 4 |
return Y.derived().template middleCols<3>(Constraint::ANGULAR) * cru.axis(); |
421 |
} |
||
422 |
}; |
||
423 |
} // namespace impl |
||
424 |
|||
425 |
template<typename Scalar, int Options> struct JointRevoluteUnalignedTpl; |
||
426 |
|||
427 |
template<typename _Scalar, int _Options> |
||
428 |
struct traits< JointRevoluteUnalignedTpl<_Scalar,_Options> > |
||
429 |
{ |
||
430 |
enum { |
||
431 |
NQ = 1, |
||
432 |
NV = 1 |
||
433 |
}; |
||
434 |
typedef _Scalar Scalar; |
||
435 |
enum { Options = _Options }; |
||
436 |
typedef JointDataRevoluteUnalignedTpl<Scalar,Options> JointDataDerived; |
||
437 |
typedef JointModelRevoluteUnalignedTpl<Scalar,Options> JointModelDerived; |
||
438 |
typedef ConstraintRevoluteUnalignedTpl<Scalar,Options> Constraint_t; |
||
439 |
typedef SE3Tpl<Scalar,Options> Transformation_t; |
||
440 |
typedef MotionRevoluteUnalignedTpl<Scalar,Options> Motion_t; |
||
441 |
typedef MotionZeroTpl<Scalar,Options> Bias_t; |
||
442 |
|||
443 |
// [ABA] |
||
444 |
typedef Eigen::Matrix<Scalar,6,NV,Options> U_t; |
||
445 |
typedef Eigen::Matrix<Scalar,NV,NV,Options> D_t; |
||
446 |
typedef Eigen::Matrix<Scalar,6,NV,Options> UD_t; |
||
447 |
|||
448 |
PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE |
||
449 |
|||
450 |
typedef Eigen::Matrix<Scalar,NQ,1,Options> ConfigVector_t; |
||
451 |
typedef Eigen::Matrix<Scalar,NV,1,Options> TangentVector_t; |
||
452 |
|||
453 |
}; |
||
454 |
|||
455 |
template<typename Scalar, int Options> |
||
456 |
struct traits< JointDataRevoluteUnalignedTpl<Scalar,Options> > |
||
457 |
{ typedef JointRevoluteUnalignedTpl<Scalar,Options> JointDerived; }; |
||
458 |
|||
459 |
template<typename Scalar, int Options> |
||
460 |
struct traits <JointModelRevoluteUnalignedTpl<Scalar,Options> > |
||
461 |
{ typedef JointRevoluteUnalignedTpl<Scalar,Options> JointDerived; }; |
||
462 |
|||
463 |
template<typename _Scalar, int _Options> |
||
464 |
struct JointDataRevoluteUnalignedTpl |
||
465 |
: public JointDataBase< JointDataRevoluteUnalignedTpl<_Scalar,_Options> > |
||
466 |
{ |
||
467 |
38 |
EIGEN_MAKE_ALIGNED_OPERATOR_NEW |
|
468 |
typedef JointRevoluteUnalignedTpl<_Scalar,_Options> JointDerived; |
||
469 |
PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(JointDerived); |
||
470 |
666 |
PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR |
|
471 |
|||
472 |
Transformation_t M; |
||
473 |
Constraint_t S; |
||
474 |
Motion_t v; |
||
475 |
Bias_t c; |
||
476 |
|||
477 |
// [ABA] specific data |
||
478 |
U_t U; |
||
479 |
D_t Dinv; |
||
480 |
UD_t UDinv; |
||
481 |
|||
482 |
26 |
JointDataRevoluteUnalignedTpl() |
|
483 |
: M(Transformation_t::Identity()) |
||
484 |
, S(Constraint_t::Vector3::Zero()) |
||
485 |
, v(Constraint_t::Vector3::Zero(),(Scalar)0) |
||
486 |
, U(U_t::Zero()) |
||
487 |
, Dinv(D_t::Zero()) |
||
488 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
26 |
, UDinv(UD_t::Zero()) |
489 |
26 |
{} |
|
490 |
|||
491 |
template<typename Vector3Like> |
||
492 |
98 |
JointDataRevoluteUnalignedTpl(const Eigen::MatrixBase<Vector3Like> & axis) |
|
493 |
: M(Transformation_t::Identity()) |
||
494 |
, S(axis) |
||
495 |
, v(axis,(Scalar)NAN) |
||
496 |
, U(U_t::Zero()) |
||
497 |
, Dinv(D_t::Zero()) |
||
498 |
✓✗✓✗ ✓✗✓✗ |
98 |
, UDinv(UD_t::Zero()) |
499 |
98 |
{} |
|
500 |
|||
501 |
✓✗ | 44 |
static std::string classname() { return std::string("JointDataRevoluteUnaligned"); } |
502 |
3 |
std::string shortname() const { return classname(); } |
|
503 |
|||
504 |
}; // struct JointDataRevoluteUnalignedTpl |
||
505 |
|||
506 |
PINOCCHIO_JOINT_CAST_TYPE_SPECIALIZATION(JointModelRevoluteUnalignedTpl); |
||
507 |
template<typename _Scalar, int _Options> |
||
508 |
struct JointModelRevoluteUnalignedTpl |
||
509 |
: public JointModelBase< JointModelRevoluteUnalignedTpl<_Scalar,_Options> > |
||
510 |
{ |
||
511 |
146 |
EIGEN_MAKE_ALIGNED_OPERATOR_NEW |
|
512 |
typedef JointRevoluteUnalignedTpl<_Scalar,_Options> JointDerived; |
||
513 |
PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived); |
||
514 |
typedef Eigen::Matrix<Scalar,3,1,_Options> Vector3; |
||
515 |
|||
516 |
typedef JointModelBase<JointModelRevoluteUnalignedTpl> Base; |
||
517 |
using Base::id; |
||
518 |
using Base::idx_q; |
||
519 |
using Base::idx_v; |
||
520 |
using Base::setIndexes; |
||
521 |
|||
522 |
64 |
JointModelRevoluteUnalignedTpl() {} |
|
523 |
|||
524 |
24 |
JointModelRevoluteUnalignedTpl(const Scalar & x, |
|
525 |
const Scalar & y, |
||
526 |
const Scalar & z) |
||
527 |
24 |
: axis(x,y,z) |
|
528 |
{ |
||
529 |
24 |
axis.normalize(); |
|
530 |
✓✗✓✗ |
24 |
assert(isUnitary(axis) && "Rotation axis is not unitary"); |
531 |
24 |
} |
|
532 |
|||
533 |
template<typename Vector3Like> |
||
534 |
40 |
JointModelRevoluteUnalignedTpl(const Eigen::MatrixBase<Vector3Like> & axis) |
|
535 |
40 |
: axis(axis) |
|
536 |
{ |
||
537 |
EIGEN_STATIC_ASSERT_VECTOR_ONLY(Vector3Like); |
||
538 |
✓✗✓✗ |
40 |
assert(isUnitary(axis) && "Rotation axis is not unitary"); |
539 |
40 |
} |
|
540 |
|||
541 |
98 |
JointDataDerived createData() const { return JointDataDerived(axis); } |
|
542 |
|||
543 |
using Base::isEqual; |
||
544 |
15 |
bool isEqual(const JointModelRevoluteUnalignedTpl & other) const |
|
545 |
{ |
||
546 |
✓✓✓✗ |
15 |
return Base::isEqual(other) && axis == other.axis; |
547 |
} |
||
548 |
|||
549 |
const std::vector<bool> hasConfigurationLimit() const |
||
550 |
{ |
||
551 |
return {true}; |
||
552 |
} |
||
553 |
|||
554 |
const std::vector<bool> hasConfigurationLimitInTangent() const |
||
555 |
{ |
||
556 |
return {true}; |
||
557 |
} |
||
558 |
|||
559 |
template<typename ConfigVector> |
||
560 |
150 |
void calc(JointDataDerived & data, |
|
561 |
const typename Eigen::MatrixBase<ConfigVector> & qs) const |
||
562 |
{ |
||
563 |
typedef typename ConfigVector::Scalar OtherScalar; |
||
564 |
typedef Eigen::AngleAxis<Scalar> AngleAxis; |
||
565 |
|||
566 |
150 |
const OtherScalar & q = qs[idx_q()]; |
|
567 |
|||
568 |
✓✗✓✗ |
150 |
data.M.rotation(AngleAxis(q,axis).toRotationMatrix()); |
569 |
150 |
} |
|
570 |
|||
571 |
template<typename ConfigVector, typename TangentVector> |
||
572 |
39 |
void calc(JointDataDerived & data, |
|
573 |
const typename Eigen::MatrixBase<ConfigVector> & qs, |
||
574 |
const typename Eigen::MatrixBase<TangentVector> & vs) const |
||
575 |
{ |
||
576 |
39 |
calc(data,qs.derived()); |
|
577 |
|||
578 |
39 |
data.v.angularRate() = static_cast<Scalar>(vs[idx_v()]); |
|
579 |
39 |
} |
|
580 |
|||
581 |
template<typename Matrix6Like> |
||
582 |
8 |
void calc_aba(JointDataDerived & data, const Eigen::MatrixBase<Matrix6Like> & I, const bool update_I) const |
|
583 |
{ |
||
584 |
✓✗✓✗ ✓✗ |
8 |
data.U.noalias() = I.template middleCols<3>(Motion::ANGULAR) * axis; |
585 |
✓✗✓✗ |
8 |
data.Dinv[0] = (Scalar)(1)/axis.dot(data.U.template segment<3>(Motion::ANGULAR)); |
586 |
✓✗✓✗ |
8 |
data.UDinv.noalias() = data.U * data.Dinv; |
587 |
|||
588 |
✓✓ | 8 |
if (update_I) |
589 |
✓✗✓✗ |
2 |
PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like,I) -= data.UDinv * data.U.transpose(); |
590 |
8 |
} |
|
591 |
|||
592 |
✓✗ | 428 |
static std::string classname() { return std::string("JointModelRevoluteUnaligned"); } |
593 |
386 |
std::string shortname() const { return classname(); } |
|
594 |
|||
595 |
/// \returns An expression of *this with the Scalar type casted to NewScalar. |
||
596 |
template<typename NewScalar> |
||
597 |
4 |
JointModelRevoluteUnalignedTpl<NewScalar,Options> cast() const |
|
598 |
{ |
||
599 |
typedef JointModelRevoluteUnalignedTpl<NewScalar,Options> ReturnType; |
||
600 |
4 |
ReturnType res(axis.template cast<NewScalar>()); |
|
601 |
4 |
res.setIndexes(id(),idx_q(),idx_v()); |
|
602 |
4 |
return res; |
|
603 |
} |
||
604 |
|||
605 |
// data |
||
606 |
|||
607 |
/// |
||
608 |
/// \brief 3d main axis of the joint. |
||
609 |
/// |
||
610 |
Vector3 axis; |
||
611 |
}; // struct JointModelRevoluteUnalignedTpl |
||
612 |
|||
613 |
} //namespace pinocchio |
||
614 |
|||
615 |
#include <boost/type_traits.hpp> |
||
616 |
|||
617 |
namespace boost |
||
618 |
{ |
||
619 |
template<typename Scalar, int Options> |
||
620 |
struct has_nothrow_constructor< ::pinocchio::JointModelRevoluteUnalignedTpl<Scalar,Options> > |
||
621 |
: public integral_constant<bool,true> {}; |
||
622 |
|||
623 |
template<typename Scalar, int Options> |
||
624 |
struct has_nothrow_copy< ::pinocchio::JointModelRevoluteUnalignedTpl<Scalar,Options> > |
||
625 |
: public integral_constant<bool,true> {}; |
||
626 |
|||
627 |
template<typename Scalar, int Options> |
||
628 |
struct has_nothrow_constructor< ::pinocchio::JointDataRevoluteUnalignedTpl<Scalar,Options> > |
||
629 |
: public integral_constant<bool,true> {}; |
||
630 |
|||
631 |
template<typename Scalar, int Options> |
||
632 |
struct has_nothrow_copy< ::pinocchio::JointDataRevoluteUnalignedTpl<Scalar,Options> > |
||
633 |
: public integral_constant<bool,true> {}; |
||
634 |
} |
||
635 |
|||
636 |
|||
637 |
#endif // ifndef __pinocchio_joint_revolute_unaligned_hpp__ |
| Generated by: GCOVR (Version 4.2) |