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_spherical_hpp__ |
||
7 |
#define __pinocchio_joint_spherical_hpp__ |
||
8 |
|||
9 |
#include "pinocchio/macros.hpp" |
||
10 |
#include "pinocchio/multibody/joint/joint-base.hpp" |
||
11 |
#include "pinocchio/multibody/constraint.hpp" |
||
12 |
#include "pinocchio/math/sincos.hpp" |
||
13 |
#include "pinocchio/spatial/inertia.hpp" |
||
14 |
#include "pinocchio/spatial/skew.hpp" |
||
15 |
|||
16 |
namespace pinocchio |
||
17 |
{ |
||
18 |
|||
19 |
template<typename Scalar, int Options = 0> struct MotionSphericalTpl; |
||
20 |
typedef MotionSphericalTpl<double> MotionSpherical; |
||
21 |
|||
22 |
template<typename Scalar, int Options> |
||
23 |
struct SE3GroupAction< MotionSphericalTpl<Scalar,Options> > |
||
24 |
{ |
||
25 |
typedef MotionTpl<Scalar,Options> ReturnType; |
||
26 |
}; |
||
27 |
|||
28 |
template<typename Scalar, int Options, typename MotionDerived> |
||
29 |
struct MotionAlgebraAction< MotionSphericalTpl<Scalar,Options>, MotionDerived> |
||
30 |
{ |
||
31 |
typedef MotionTpl<Scalar,Options> ReturnType; |
||
32 |
}; |
||
33 |
|||
34 |
template<typename _Scalar, int _Options> |
||
35 |
struct traits< MotionSphericalTpl<_Scalar,_Options> > |
||
36 |
{ |
||
37 |
typedef _Scalar Scalar; |
||
38 |
enum { Options = _Options }; |
||
39 |
typedef Eigen::Matrix<Scalar,3,1,Options> Vector3; |
||
40 |
typedef Eigen::Matrix<Scalar,6,1,Options> Vector6; |
||
41 |
typedef Eigen::Matrix<Scalar,4,4,Options> Matrix4; |
||
42 |
typedef Eigen::Matrix<Scalar,6,6,Options> Matrix6; |
||
43 |
typedef typename PINOCCHIO_EIGEN_REF_CONST_TYPE(Vector6) ToVectorConstReturnType; |
||
44 |
typedef typename PINOCCHIO_EIGEN_REF_TYPE(Vector6) ToVectorReturnType; |
||
45 |
typedef Vector3 AngularType; |
||
46 |
typedef Vector3 LinearType; |
||
47 |
typedef const Vector3 ConstAngularType; |
||
48 |
typedef const Vector3 ConstLinearType; |
||
49 |
typedef Matrix6 ActionMatrixType; |
||
50 |
typedef Matrix4 HomogeneousMatrixType; |
||
51 |
typedef MotionTpl<Scalar,Options> MotionPlain; |
||
52 |
typedef MotionPlain PlainReturnType; |
||
53 |
enum { |
||
54 |
LINEAR = 0, |
||
55 |
ANGULAR = 3 |
||
56 |
}; |
||
57 |
}; // traits MotionSphericalTpl |
||
58 |
|||
59 |
template<typename _Scalar, int _Options> |
||
60 |
struct MotionSphericalTpl |
||
61 |
: MotionBase< MotionSphericalTpl<_Scalar,_Options> > |
||
62 |
{ |
||
63 |
EIGEN_MAKE_ALIGNED_OPERATOR_NEW |
||
64 |
|||
65 |
MOTION_TYPEDEF_TPL(MotionSphericalTpl); |
||
66 |
|||
67 |
21 |
MotionSphericalTpl() {} |
|
68 |
|||
69 |
template<typename Vector3Like> |
||
70 |
9674 |
MotionSphericalTpl(const Eigen::MatrixBase<Vector3Like> & w) |
|
71 |
9674 |
: m_w(w) |
|
72 |
9674 |
{} |
|
73 |
|||
74 |
4140 |
Vector3 & operator() () { return m_w; } |
|
75 |
const Vector3 & operator() () const { return m_w; } |
||
76 |
|||
77 |
2 |
inline PlainReturnType plain() const |
|
78 |
{ |
||
79 |
✓✗ | 2 |
return PlainReturnType(PlainReturnType::Vector3::Zero(), m_w); |
80 |
} |
||
81 |
|||
82 |
template<typename MotionDerived> |
||
83 |
8 |
void addTo(MotionDense<MotionDerived> & other) const |
|
84 |
{ |
||
85 |
✓✗ | 8 |
other.angular() += m_w; |
86 |
8 |
} |
|
87 |
|||
88 |
template<typename Derived> |
||
89 |
2200 |
void setTo(MotionDense<Derived> & other) const |
|
90 |
{ |
||
91 |
✓✗ | 2200 |
other.linear().setZero(); |
92 |
✓✗ | 2200 |
other.angular() = m_w; |
93 |
2200 |
} |
|
94 |
|||
95 |
MotionSphericalTpl __plus__(const MotionSphericalTpl & other) const |
||
96 |
{ |
||
97 |
return MotionSphericalTpl(m_w + other.m_w); |
||
98 |
} |
||
99 |
|||
100 |
51 |
bool isEqual_impl(const MotionSphericalTpl & other) const |
|
101 |
{ |
||
102 |
51 |
return m_w == other.m_w; |
|
103 |
} |
||
104 |
|||
105 |
template<typename MotionDerived> |
||
106 |
1 |
bool isEqual_impl(const MotionDense<MotionDerived> & other) const |
|
107 |
{ |
||
108 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
1 |
return other.angular() == m_w && other.linear().isZero(0); |
109 |
} |
||
110 |
|||
111 |
template<typename S2, int O2, typename D2> |
||
112 |
2 |
void se3Action_impl(const SE3Tpl<S2,O2> & m, MotionDense<D2> & v) const |
|
113 |
{ |
||
114 |
// Angular |
||
115 |
✓✗✓✗ ✓✗ |
2 |
v.angular().noalias() = m.rotation() * m_w; |
116 |
|||
117 |
// Linear |
||
118 |
✓✗✓✗ ✓✗✓✗ |
2 |
v.linear().noalias() = m.translation().cross(v.angular()); |
119 |
2 |
} |
|
120 |
|||
121 |
template<typename S2, int O2> |
||
122 |
2 |
MotionPlain se3Action_impl(const SE3Tpl<S2,O2> & m) const |
|
123 |
{ |
||
124 |
2 |
MotionPlain res; |
|
125 |
2 |
se3Action_impl(m,res); |
|
126 |
2 |
return res; |
|
127 |
} |
||
128 |
|||
129 |
template<typename S2, int O2, typename D2> |
||
130 |
2 |
void se3ActionInverse_impl(const SE3Tpl<S2,O2> & m, MotionDense<D2> & v) const |
|
131 |
{ |
||
132 |
// Linear |
||
133 |
// TODO: use v.angular() as temporary variable |
||
134 |
✓✗ | 2 |
Vector3 v3_tmp; |
135 |
✓✗✓✗ ✓✗✓✗ |
2 |
v3_tmp.noalias() = m_w.cross(m.translation()); |
136 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
2 |
v.linear().noalias() = m.rotation().transpose() * v3_tmp; |
137 |
|||
138 |
// Angular |
||
139 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
2 |
v.angular().noalias() = m.rotation().transpose() * m_w; |
140 |
2 |
} |
|
141 |
|||
142 |
template<typename S2, int O2> |
||
143 |
2 |
MotionPlain se3ActionInverse_impl(const SE3Tpl<S2,O2> & m) const |
|
144 |
{ |
||
145 |
2 |
MotionPlain res; |
|
146 |
2 |
se3ActionInverse_impl(m,res); |
|
147 |
2 |
return res; |
|
148 |
} |
||
149 |
|||
150 |
template<typename M1, typename M2> |
||
151 |
124 |
void motionAction(const MotionDense<M1> & v, MotionDense<M2> & mout) const |
|
152 |
{ |
||
153 |
// Linear |
||
154 |
✓✗✓✗ ✓✗✓✗ |
124 |
mout.linear().noalias() = v.linear().cross(m_w); |
155 |
|||
156 |
// Angular |
||
157 |
✓✗✓✗ ✓✗✓✗ |
124 |
mout.angular().noalias() = v.angular().cross(m_w); |
158 |
124 |
} |
|
159 |
|||
160 |
template<typename M1> |
||
161 |
124 |
MotionPlain motionAction(const MotionDense<M1> & v) const |
|
162 |
{ |
||
163 |
124 |
MotionPlain res; |
|
164 |
124 |
motionAction(v,res); |
|
165 |
124 |
return res; |
|
166 |
} |
||
167 |
|||
168 |
116 |
const Vector3 & angular() const { return m_w; } |
|
169 |
1244 |
Vector3 & angular() { return m_w; } |
|
170 |
|||
171 |
protected: |
||
172 |
|||
173 |
Vector3 m_w; |
||
174 |
}; // struct MotionSphericalTpl |
||
175 |
|||
176 |
template<typename S1, int O1, typename MotionDerived> |
||
177 |
inline typename MotionDerived::MotionPlain |
||
178 |
116 |
operator+(const MotionSphericalTpl<S1,O1> & m1, const MotionDense<MotionDerived> & m2) |
|
179 |
{ |
||
180 |
✓✗✓✗ ✓✗ |
116 |
return typename MotionDerived::MotionPlain(m2.linear(),m2.angular() + m1.angular()); |
181 |
} |
||
182 |
|||
183 |
template<typename Scalar, int Options> struct ConstraintSphericalTpl; |
||
184 |
|||
185 |
template<typename _Scalar, int _Options> |
||
186 |
struct traits< ConstraintSphericalTpl<_Scalar,_Options> > |
||
187 |
{ |
||
188 |
typedef _Scalar Scalar; |
||
189 |
enum { Options = _Options }; |
||
190 |
enum { |
||
191 |
LINEAR = 0, |
||
192 |
ANGULAR = 3 |
||
193 |
}; |
||
194 |
typedef MotionSphericalTpl<Scalar,Options> JointMotion; |
||
195 |
typedef Eigen::Matrix<Scalar,3,1,Options> JointForce; |
||
196 |
typedef Eigen::Matrix<Scalar,6,3,Options> DenseBase; |
||
197 |
typedef DenseBase MatrixReturnType; |
||
198 |
typedef const DenseBase ConstMatrixReturnType; |
||
199 |
}; // struct traits struct ConstraintSphericalTpl |
||
200 |
|||
201 |
template<typename _Scalar, int _Options> |
||
202 |
struct ConstraintSphericalTpl |
||
203 |
: public ConstraintBase< ConstraintSphericalTpl<_Scalar,_Options> > |
||
204 |
{ |
||
205 |
EIGEN_MAKE_ALIGNED_OPERATOR_NEW |
||
206 |
|||
207 |
PINOCCHIO_CONSTRAINT_TYPEDEF_TPL(ConstraintSphericalTpl) |
||
208 |
|||
209 |
3145 |
ConstraintSphericalTpl() {} |
|
210 |
|||
211 |
enum { NV = 3 }; |
||
212 |
|||
213 |
22 |
int nv_impl() const { return NV; } |
|
214 |
|||
215 |
template<typename Vector3Like> |
||
216 |
126 |
JointMotion __mult__(const Eigen::MatrixBase<Vector3Like> & w) const |
|
217 |
{ |
||
218 |
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3Like,3); |
||
219 |
126 |
return JointMotion(w); |
|
220 |
} |
||
221 |
|||
222 |
struct TransposeConst |
||
223 |
{ |
||
224 |
template<typename Derived> |
||
225 |
typename ForceDense<Derived>::ConstAngularType |
||
226 |
7 |
operator* (const ForceDense<Derived> & phi) |
|
227 |
7 |
{ return phi.angular(); } |
|
228 |
|||
229 |
/* [CRBA] MatrixBase operator* (Constraint::Transpose S, ForceSet::Block) */ |
||
230 |
template<typename MatrixDerived> |
||
231 |
const typename SizeDepType<3>::RowsReturn<MatrixDerived>::ConstType |
||
232 |
7 |
operator*(const Eigen::MatrixBase<MatrixDerived> & F) const |
|
233 |
{ |
||
234 |
✗✓ | 7 |
assert(F.rows()==6); |
235 |
7 |
return F.derived().template middleRows<3>(Inertia::ANGULAR); |
|
236 |
} |
||
237 |
}; |
||
238 |
|||
239 |
14 |
TransposeConst transpose() const { return TransposeConst(); } |
|
240 |
|||
241 |
21 |
DenseBase matrix_impl() const |
|
242 |
{ |
||
243 |
21 |
DenseBase S; |
|
244 |
✓✗ | 21 |
S.template block <3,3>(LINEAR,0).setZero(); |
245 |
✓✗ | 21 |
S.template block <3,3>(ANGULAR,0).setIdentity(); |
246 |
21 |
return S; |
|
247 |
} |
||
248 |
|||
249 |
template<typename S1, int O1> |
||
250 |
54 |
Eigen::Matrix<S1,6,3,O1> se3Action(const SE3Tpl<S1,O1> & m) const |
|
251 |
{ |
||
252 |
54 |
Eigen::Matrix<S1,6,3,O1> X_subspace; |
|
253 |
✓✗✓✗ ✓✗ |
54 |
cross(m.translation(),m.rotation(),X_subspace.template middleRows<3>(LINEAR)); |
254 |
✓✗ | 54 |
X_subspace.template middleRows<3>(ANGULAR) = m.rotation(); |
255 |
|||
256 |
54 |
return X_subspace; |
|
257 |
} |
||
258 |
|||
259 |
template<typename S1, int O1> |
||
260 |
3 |
Eigen::Matrix<S1,6,3,O1> se3ActionInverse(const SE3Tpl<S1,O1> & m) const |
|
261 |
{ |
||
262 |
3 |
Eigen::Matrix<S1,6,3,O1> X_subspace; |
|
263 |
✓✗✓✗ ✓✗ |
3 |
AxisX::cross(m.translation(),X_subspace.template middleRows<3>(ANGULAR).col(0)); |
264 |
✓✗✓✗ ✓✗ |
3 |
AxisY::cross(m.translation(),X_subspace.template middleRows<3>(ANGULAR).col(1)); |
265 |
✓✗✓✗ ✓✗ |
3 |
AxisZ::cross(m.translation(),X_subspace.template middleRows<3>(ANGULAR).col(2)); |
266 |
|||
267 |
X_subspace.template middleRows<3>(LINEAR).noalias() |
||
268 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
3 |
= m.rotation().transpose() * X_subspace.template middleRows<3>(ANGULAR); |
269 |
✓✗✓✗ |
3 |
X_subspace.template middleRows<3>(ANGULAR) = m.rotation().transpose(); |
270 |
|||
271 |
3 |
return X_subspace; |
|
272 |
} |
||
273 |
|||
274 |
template<typename MotionDerived> |
||
275 |
2 |
DenseBase motionAction(const MotionDense<MotionDerived> & m) const |
|
276 |
{ |
||
277 |
✓✗ | 2 |
const typename MotionDerived::ConstLinearType v = m.linear(); |
278 |
✓✗ | 2 |
const typename MotionDerived::ConstAngularType w = m.angular(); |
279 |
|||
280 |
✓✗ | 2 |
DenseBase res; |
281 |
✓✗✓✗ |
2 |
skew(v,res.template middleRows<3>(LINEAR)); |
282 |
✓✗✓✗ |
2 |
skew(w,res.template middleRows<3>(ANGULAR)); |
283 |
|||
284 |
4 |
return res; |
|
285 |
} |
||
286 |
|||
287 |
17 |
bool isEqual(const ConstraintSphericalTpl &) const { return true; } |
|
288 |
|||
289 |
}; // struct ConstraintSphericalTpl |
||
290 |
|||
291 |
template<typename MotionDerived, typename S2, int O2> |
||
292 |
inline typename MotionDerived::MotionPlain |
||
293 |
122 |
operator^(const MotionDense<MotionDerived> & m1, |
|
294 |
const MotionSphericalTpl<S2,O2> & m2) |
||
295 |
{ |
||
296 |
122 |
return m2.motionAction(m1); |
|
297 |
} |
||
298 |
|||
299 |
/* [CRBA] ForceSet operator* (Inertia Y,Constraint S) */ |
||
300 |
template<typename S1, int O1, typename S2, int O2> |
||
301 |
inline Eigen::Matrix<S2,6,3,O2> |
||
302 |
8 |
operator*(const InertiaTpl<S1,O1> & Y, |
|
303 |
const ConstraintSphericalTpl<S2,O2> &) |
||
304 |
{ |
||
305 |
typedef InertiaTpl<S1,O1> Inertia; |
||
306 |
typedef typename Inertia::Symmetric3 Symmetric3; |
||
307 |
8 |
Eigen::Matrix<S2,6,3,O2> M; |
|
308 |
// M.block <3,3> (Inertia::LINEAR, 0) = - Y.mass () * skew(Y.lever ()); |
||
309 |
✓✗✓✗ |
8 |
M.template block<3,3>(Inertia::LINEAR,0) = alphaSkew(-Y.mass(), Y.lever()); |
310 |
✓✗✓✗ ✓✗✓✗ |
8 |
M.template block<3,3>(Inertia::ANGULAR,0) = (Y.inertia() - typename Symmetric3::AlphaSkewSquare(Y.mass(), Y.lever())).matrix(); |
311 |
8 |
return M; |
|
312 |
} |
||
313 |
|||
314 |
/* [ABA] Y*S operator*/ |
||
315 |
template<typename M6Like, typename S2, int O2> |
||
316 |
inline typename SizeDepType<3>::ColsReturn<M6Like>::ConstType |
||
317 |
2 |
operator*(const Eigen::MatrixBase<M6Like> & Y, |
|
318 |
const ConstraintSphericalTpl<S2,O2> &) |
||
319 |
{ |
||
320 |
typedef ConstraintSphericalTpl<S2,O2> Constraint; |
||
321 |
2 |
return Y.derived().template middleCols<3>(Constraint::ANGULAR); |
|
322 |
} |
||
323 |
|||
324 |
template<typename S1, int O1> |
||
325 |
struct SE3GroupAction< ConstraintSphericalTpl<S1,O1> > |
||
326 |
{ typedef Eigen::Matrix<S1,6,3,O1> ReturnType; }; |
||
327 |
|||
328 |
template<typename S1, int O1, typename MotionDerived> |
||
329 |
struct MotionAlgebraAction< ConstraintSphericalTpl<S1,O1>,MotionDerived > |
||
330 |
{ typedef Eigen::Matrix<S1,6,3,O1> ReturnType; }; |
||
331 |
|||
332 |
template<typename Scalar, int Options> struct JointSphericalTpl; |
||
333 |
|||
334 |
template<typename _Scalar, int _Options> |
||
335 |
struct traits< JointSphericalTpl<_Scalar,_Options> > |
||
336 |
{ |
||
337 |
enum { |
||
338 |
NQ = 4, |
||
339 |
NV = 3 |
||
340 |
}; |
||
341 |
typedef _Scalar Scalar; |
||
342 |
enum { Options = _Options }; |
||
343 |
typedef JointDataSphericalTpl<Scalar,Options> JointDataDerived; |
||
344 |
typedef JointModelSphericalTpl<Scalar,Options> JointModelDerived; |
||
345 |
typedef ConstraintSphericalTpl<Scalar,Options> Constraint_t; |
||
346 |
typedef SE3Tpl<Scalar,Options> Transformation_t; |
||
347 |
typedef MotionSphericalTpl<Scalar,Options> Motion_t; |
||
348 |
typedef MotionZeroTpl<Scalar,Options> Bias_t; |
||
349 |
|||
350 |
// [ABA] |
||
351 |
typedef Eigen::Matrix<Scalar,6,NV,Options> U_t; |
||
352 |
typedef Eigen::Matrix<Scalar,NV,NV,Options> D_t; |
||
353 |
typedef Eigen::Matrix<Scalar,6,NV,Options> UD_t; |
||
354 |
|||
355 |
PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE |
||
356 |
|||
357 |
typedef Eigen::Matrix<Scalar,NQ,1,Options> ConfigVector_t; |
||
358 |
typedef Eigen::Matrix<Scalar,NV,1,Options> TangentVector_t; |
||
359 |
}; |
||
360 |
|||
361 |
template<typename Scalar, int Options> |
||
362 |
struct traits< JointDataSphericalTpl<Scalar,Options> > |
||
363 |
{ typedef JointSphericalTpl<Scalar,Options> JointDerived; }; |
||
364 |
|||
365 |
template<typename Scalar, int Options> |
||
366 |
struct traits< JointModelSphericalTpl<Scalar,Options> > |
||
367 |
{ typedef JointSphericalTpl<Scalar,Options> JointDerived; }; |
||
368 |
|||
369 |
template<typename _Scalar, int _Options> |
||
370 |
struct JointDataSphericalTpl : public JointDataBase< JointDataSphericalTpl<_Scalar,_Options> > |
||
371 |
{ |
||
372 |
108 |
EIGEN_MAKE_ALIGNED_OPERATOR_NEW |
|
373 |
|||
374 |
typedef JointSphericalTpl<_Scalar,_Options> JointDerived; |
||
375 |
PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(JointDerived); |
||
376 |
2088 |
PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR |
|
377 |
|||
378 |
Constraint_t S; |
||
379 |
Transformation_t M; |
||
380 |
Motion_t v; |
||
381 |
Bias_t c; |
||
382 |
|||
383 |
// [ABA] specific data |
||
384 |
U_t U; |
||
385 |
D_t Dinv; |
||
386 |
UD_t UDinv; |
||
387 |
|||
388 |
3138 |
JointDataSphericalTpl () |
|
389 |
: M(Transformation_t::Identity()) |
||
390 |
, v(Motion_t::Vector3::Zero()) |
||
391 |
, U(U_t::Zero()) |
||
392 |
, Dinv(D_t::Zero()) |
||
393 |
✓✗✓✗ ✓✗✓✗ |
3138 |
, UDinv(UD_t::Zero()) |
394 |
3138 |
{} |
|
395 |
|||
396 |
✓✗ | 44 |
static std::string classname() { return std::string("JointDataSpherical"); } |
397 |
3 |
std::string shortname() const { return classname(); } |
|
398 |
|||
399 |
}; // struct JointDataSphericalTpl |
||
400 |
|||
401 |
PINOCCHIO_JOINT_CAST_TYPE_SPECIALIZATION(JointModelSphericalTpl); |
||
402 |
template<typename _Scalar, int _Options> |
||
403 |
struct JointModelSphericalTpl |
||
404 |
: public JointModelBase< JointModelSphericalTpl<_Scalar,_Options> > |
||
405 |
{ |
||
406 |
204 |
EIGEN_MAKE_ALIGNED_OPERATOR_NEW |
|
407 |
|||
408 |
typedef JointSphericalTpl<_Scalar,_Options> JointDerived; |
||
409 |
PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived); |
||
410 |
|||
411 |
typedef JointModelBase<JointModelSphericalTpl> Base; |
||
412 |
using Base::id; |
||
413 |
using Base::idx_q; |
||
414 |
using Base::idx_v; |
||
415 |
using Base::setIndexes; |
||
416 |
|||
417 |
3112 |
JointDataDerived createData() const { return JointDataDerived(); } |
|
418 |
|||
419 |
const std::vector<bool> hasConfigurationLimit() const |
||
420 |
{ |
||
421 |
return {false, false, false, false}; |
||
422 |
} |
||
423 |
|||
424 |
const std::vector<bool> hasConfigurationLimitInTangent() const |
||
425 |
{ |
||
426 |
return {false, false, false}; |
||
427 |
} |
||
428 |
|||
429 |
template<typename ConfigVectorLike> |
||
430 |
inline void forwardKinematics(Transformation_t & M, const Eigen::MatrixBase<ConfigVectorLike> & q_joint) const |
||
431 |
{ |
||
432 |
typedef typename ConfigVectorLike::Scalar Scalar; |
||
433 |
EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigVector_t,ConfigVectorLike); |
||
434 |
typedef typename Eigen::Quaternion<Scalar,PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorLike)::Options> Quaternion; |
||
435 |
typedef Eigen::Map<const Quaternion> ConstQuaternionMap; |
||
436 |
|||
437 |
ConstQuaternionMap quat(q_joint.derived().data()); |
||
438 |
//assert(math::fabs(quat.coeffs().squaredNorm()-1.) <= sqrt(Eigen::NumTraits<typename V::Scalar>::epsilon())); TODO: check validity of the rhs precision |
||
439 |
assert(math::fabs(static_cast<Scalar>(quat.coeffs().squaredNorm()-1)) <= 1e-4); |
||
440 |
|||
441 |
M.rotation(quat.matrix()); |
||
442 |
M.translation().setZero(); |
||
443 |
} |
||
444 |
|||
445 |
template<typename QuaternionDerived> |
||
446 |
6319 |
void calc(JointDataDerived & data, |
|
447 |
const typename Eigen::QuaternionBase<QuaternionDerived> & quat) const |
||
448 |
{ |
||
449 |
✓✗ | 6319 |
data.M.rotation(quat.matrix()); |
450 |
6319 |
} |
|
451 |
|||
452 |
template<typename ConfigVector> |
||
453 |
EIGEN_DONT_INLINE |
||
454 |
6331 |
void calc(JointDataDerived & data, |
|
455 |
const typename Eigen::PlainObjectBase<ConfigVector> & qs) const |
||
456 |
{ |
||
457 |
typedef typename Eigen::Quaternion<typename ConfigVector::Scalar,ConfigVector::Options> Quaternion; |
||
458 |
typedef Eigen::Map<const Quaternion> ConstQuaternionMap; |
||
459 |
|||
460 |
✓✗✓✗ ✓✗ |
6331 |
ConstQuaternionMap quat(qs.template segment<NQ>(idx_q()).data()); |
461 |
✓✗ | 6331 |
calc(data,quat); |
462 |
6331 |
} |
|
463 |
|||
464 |
template<typename ConfigVector> |
||
465 |
EIGEN_DONT_INLINE |
||
466 |
void calc(JointDataDerived & data, |
||
467 |
const typename Eigen::MatrixBase<ConfigVector> & qs) const |
||
468 |
{ |
||
469 |
typedef typename Eigen::Quaternion<Scalar,Options> Quaternion; |
||
470 |
|||
471 |
const Quaternion quat(qs.template segment<NQ>(idx_q())); |
||
472 |
calc(data,quat); |
||
473 |
} |
||
474 |
|||
475 |
template<typename ConfigVector, typename TangentVector> |
||
476 |
1182 |
void calc(JointDataDerived & data, |
|
477 |
const typename Eigen::MatrixBase<ConfigVector> & qs, |
||
478 |
const typename Eigen::MatrixBase<TangentVector> & vs) const |
||
479 |
{ |
||
480 |
1182 |
calc(data,qs.derived()); |
|
481 |
|||
482 |
✓✗ | 1182 |
data.v.angular() = vs.template segment<NV>(idx_v()); |
483 |
1182 |
} |
|
484 |
|||
485 |
template<typename Matrix6Like> |
||
486 |
8 |
void calc_aba(JointDataDerived & data, |
|
487 |
const Eigen::MatrixBase<Matrix6Like> & I, |
||
488 |
const bool update_I) const |
||
489 |
{ |
||
490 |
✓✗ | 8 |
data.U = I.template block<6,3>(0,Inertia::ANGULAR); |
491 |
|||
492 |
// compute inverse |
||
493 |
// data.Dinv.setIdentity(); |
||
494 |
// data.U.template middleRows<3>(Inertia::ANGULAR).llt().solveInPlace(data.Dinv); |
||
495 |
✓✗ | 8 |
internal::PerformStYSInversion<Scalar>::run(data.U.template middleRows<3>(Inertia::ANGULAR),data.Dinv); |
496 |
|||
497 |
✓✗ | 8 |
data.UDinv.template middleRows<3>(Inertia::ANGULAR).setIdentity(); // can be put in data constructor |
498 |
✓✗✓✗ ✓✗✓✗ |
8 |
data.UDinv.template middleRows<3>(Inertia::LINEAR).noalias() = data.U.template block<3,3>(Inertia::LINEAR, 0) * data.Dinv; |
499 |
|||
500 |
✓✓ | 8 |
if (update_I) |
501 |
{ |
||
502 |
2 |
Matrix6Like & I_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like,I); |
|
503 |
I_.template block<3,3>(Inertia::LINEAR,Inertia::LINEAR) |
||
504 |
✓✗✓✗ ✓✗✓✗ |
2 |
-= data.UDinv.template middleRows<3>(Inertia::LINEAR) * I_.template block<3,3> (Inertia::ANGULAR, Inertia::LINEAR); |
505 |
✓✗ | 2 |
I_.template block<6,3>(0,Inertia::ANGULAR).setZero(); |
506 |
✓✗ | 2 |
I_.template block<3,3>(Inertia::ANGULAR,Inertia::LINEAR).setZero(); |
507 |
} |
||
508 |
8 |
} |
|
509 |
|||
510 |
✓✗ | 15368 |
static std::string classname() { return std::string("JointModelSpherical"); } |
511 |
15326 |
std::string shortname() const { return classname(); } |
|
512 |
|||
513 |
/// \returns An expression of *this with the Scalar type casted to NewScalar. |
||
514 |
template<typename NewScalar> |
||
515 |
4 |
JointModelSphericalTpl<NewScalar,Options> cast() const |
|
516 |
{ |
||
517 |
typedef JointModelSphericalTpl<NewScalar,Options> ReturnType; |
||
518 |
4 |
ReturnType res; |
|
519 |
4 |
res.setIndexes(id(),idx_q(),idx_v()); |
|
520 |
4 |
return res; |
|
521 |
} |
||
522 |
|||
523 |
}; // struct JointModelSphericalTpl |
||
524 |
|||
525 |
} // namespace pinocchio |
||
526 |
|||
527 |
#include <boost/type_traits.hpp> |
||
528 |
|||
529 |
namespace boost |
||
530 |
{ |
||
531 |
template<typename Scalar, int Options> |
||
532 |
struct has_nothrow_constructor< ::pinocchio::JointModelSphericalTpl<Scalar,Options> > |
||
533 |
: public integral_constant<bool,true> {}; |
||
534 |
|||
535 |
template<typename Scalar, int Options> |
||
536 |
struct has_nothrow_copy< ::pinocchio::JointModelSphericalTpl<Scalar,Options> > |
||
537 |
: public integral_constant<bool,true> {}; |
||
538 |
|||
539 |
template<typename Scalar, int Options> |
||
540 |
struct has_nothrow_constructor< ::pinocchio::JointDataSphericalTpl<Scalar,Options> > |
||
541 |
: public integral_constant<bool,true> {}; |
||
542 |
|||
543 |
template<typename Scalar, int Options> |
||
544 |
struct has_nothrow_copy< ::pinocchio::JointDataSphericalTpl<Scalar,Options> > |
||
545 |
: public integral_constant<bool,true> {}; |
||
546 |
} |
||
547 |
|||
548 |
#endif // ifndef __pinocchio_joint_spherical_hpp__ |
Generated by: GCOVR (Version 4.2) |