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_ZYX_hpp__ |
||
7 |
#define __pinocchio_joint_spherical_ZYX_hpp__ |
||
8 |
|||
9 |
#include "pinocchio/macros.hpp" |
||
10 |
#include "pinocchio/multibody/joint/joint-base.hpp" |
||
11 |
#include "pinocchio/multibody/joint/joint-spherical.hpp" |
||
12 |
#include "pinocchio/multibody/constraint.hpp" |
||
13 |
#include "pinocchio/math/sincos.hpp" |
||
14 |
#include "pinocchio/math/matrix.hpp" |
||
15 |
#include "pinocchio/spatial/inertia.hpp" |
||
16 |
#include "pinocchio/spatial/skew.hpp" |
||
17 |
|||
18 |
namespace pinocchio |
||
19 |
{ |
||
20 |
template<typename Scalar, int Options> struct ConstraintSphericalZYXTpl; |
||
21 |
|||
22 |
template <typename _Scalar, int _Options> |
||
23 |
struct traits< ConstraintSphericalZYXTpl<_Scalar,_Options> > |
||
24 |
{ |
||
25 |
typedef _Scalar Scalar; |
||
26 |
enum { Options = _Options }; |
||
27 |
|||
28 |
enum { |
||
29 |
LINEAR = 0, |
||
30 |
ANGULAR = 3 |
||
31 |
}; |
||
32 |
|||
33 |
typedef MotionSphericalTpl<Scalar,Options> JointMotion; |
||
34 |
typedef Eigen::Matrix<Scalar,3,1,Options> JointForce; |
||
35 |
typedef Eigen::Matrix<Scalar,6,3,Options> DenseBase; |
||
36 |
|||
37 |
typedef DenseBase MatrixReturnType; |
||
38 |
typedef const DenseBase ConstMatrixReturnType; |
||
39 |
}; // struct traits struct ConstraintRotationalSubspace |
||
40 |
|||
41 |
template<typename _Scalar, int _Options> |
||
42 |
struct ConstraintSphericalZYXTpl |
||
43 |
: public ConstraintBase< ConstraintSphericalZYXTpl<_Scalar,_Options> > |
||
44 |
{ |
||
45 |
EIGEN_MAKE_ALIGNED_OPERATOR_NEW |
||
46 |
|||
47 |
PINOCCHIO_CONSTRAINT_TYPEDEF_TPL(ConstraintSphericalZYXTpl) |
||
48 |
|||
49 |
enum { NV = 3 }; |
||
50 |
typedef Eigen::Matrix<Scalar,3,3,Options> Matrix3; |
||
51 |
|||
52 |
7 |
ConstraintSphericalZYXTpl() {} |
|
53 |
|||
54 |
template<typename Matrix3Like> |
||
55 |
3124 |
ConstraintSphericalZYXTpl(const Eigen::MatrixBase<Matrix3Like> & subspace) |
|
56 |
3124 |
: m_S(subspace) |
|
57 |
3124 |
{ EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix3Like,3,3); } |
|
58 |
|||
59 |
template<typename Vector3Like> |
||
60 |
7 |
JointMotion __mult__(const Eigen::MatrixBase<Vector3Like> & v) const |
|
61 |
{ |
||
62 |
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3Like,3); |
||
63 |
✓✗ | 8 |
return JointMotion(m_S * v); |
64 |
} |
||
65 |
|||
66 |
Matrix3 & operator() () { return m_S; } |
||
67 |
const Matrix3 & operator() () const { return m_S; } |
||
68 |
|||
69 |
22 |
int nv_impl() const { return NV; } |
|
70 |
|||
71 |
struct ConstraintTranspose |
||
72 |
{ |
||
73 |
const ConstraintSphericalZYXTpl & ref; |
||
74 |
9 |
ConstraintTranspose(const ConstraintSphericalZYXTpl & ref) : ref(ref) {} |
|
75 |
|||
76 |
template<typename Derived> |
||
77 |
const typename MatrixMatrixProduct< |
||
78 |
Eigen::Transpose<const Matrix3>, |
||
79 |
Eigen::Block<const typename ForceDense<Derived>::Vector6,3,1> |
||
80 |
>::type |
||
81 |
5 |
operator* (const ForceDense<Derived> & phi) const |
|
82 |
{ |
||
83 |
✓✗✓✗ |
10 |
return ref.m_S.transpose() * phi.angular(); |
84 |
} |
||
85 |
|||
86 |
/* [CRBA] MatrixBase operator* (Constraint::Transpose S, ForceSet::Block) */ |
||
87 |
template<typename D> |
||
88 |
const typename MatrixMatrixProduct< |
||
89 |
typename Eigen::Transpose<const Matrix3>, |
||
90 |
typename Eigen::MatrixBase<const D>::template NRowsBlockXpr<3>::Type |
||
91 |
>::type |
||
92 |
4 |
operator* (const Eigen::MatrixBase<D> & F) const |
|
93 |
{ |
||
94 |
EIGEN_STATIC_ASSERT(D::RowsAtCompileTime==6,THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE) |
||
95 |
✓✗✓✗ |
8 |
return ref.m_S.transpose () * F.template middleRows<3>(ANGULAR); |
96 |
} |
||
97 |
}; // struct ConstraintTranspose |
||
98 |
|||
99 |
9 |
ConstraintTranspose transpose () const { return ConstraintTranspose(*this); } |
|
100 |
|||
101 |
23 |
DenseBase matrix_impl() const |
|
102 |
{ |
||
103 |
23 |
DenseBase S; |
|
104 |
✓✗ | 23 |
S.template middleRows<3>(LINEAR).setZero(); |
105 |
✓✗ | 23 |
S.template middleRows<3>(ANGULAR) = m_S; |
106 |
23 |
return S; |
|
107 |
} |
||
108 |
|||
109 |
// const typename Eigen::ProductReturnType< |
||
110 |
// const ConstraintDense, |
||
111 |
// const Matrix3 |
||
112 |
// >::Type |
||
113 |
template<typename S1, int O1> |
||
114 |
Eigen::Matrix<Scalar,6,3,Options> |
||
115 |
4 |
se3Action(const SE3Tpl<S1,O1> & m) const |
|
116 |
{ |
||
117 |
// Eigen::Matrix <Scalar,6,3,Options> X_subspace; |
||
118 |
// X_subspace.template block <3,3> (Motion::LINEAR, 0) = skew (m.translation ()) * m.rotation (); |
||
119 |
// X_subspace.template block <3,3> (Motion::ANGULAR, 0) = m.rotation (); |
||
120 |
// |
||
121 |
// return (X_subspace * m_S).eval(); |
||
122 |
|||
123 |
4 |
Eigen::Matrix<Scalar,6,3,Options> result; |
|
124 |
|||
125 |
// ANGULAR |
||
126 |
✓✗✓✗ ✓✗ |
4 |
result.template middleRows<3>(ANGULAR).noalias() = m.rotation () * m_S; |
127 |
|||
128 |
// LINEAR |
||
129 |
✓✗✓✗ ✓✗ |
4 |
cross(m.translation(), |
130 |
result.template middleRows<3>(Motion::ANGULAR), |
||
131 |
result.template middleRows<3>(LINEAR)); |
||
132 |
|||
133 |
4 |
return result; |
|
134 |
} |
||
135 |
|||
136 |
template<typename S1, int O1> |
||
137 |
Eigen::Matrix<Scalar,6,3,Options> |
||
138 |
3 |
se3ActionInverse(const SE3Tpl<S1,O1> & m) const |
|
139 |
{ |
||
140 |
3 |
Eigen::Matrix<Scalar,6,3,Options> result; |
|
141 |
|||
142 |
// LINEAR |
||
143 |
✓✗ | 6 |
cross(m.translation(), |
144 |
✓✗ | 3 |
m_S, |
145 |
result.template middleRows<3>(ANGULAR)); |
||
146 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
3 |
result.template middleRows<3>(LINEAR).noalias() = -m.rotation().transpose() * result.template middleRows<3>(ANGULAR); |
147 |
|||
148 |
// ANGULAR |
||
149 |
✓✗✓✗ ✓✗✓✗ |
3 |
result.template middleRows<3>(ANGULAR).noalias() = m.rotation().transpose() * m_S; |
150 |
|||
151 |
3 |
return result; |
|
152 |
} |
||
153 |
|||
154 |
template<typename MotionDerived> |
||
155 |
2 |
DenseBase motionAction(const MotionDense<MotionDerived> & m) const |
|
156 |
{ |
||
157 |
✓✗ | 2 |
const typename MotionDerived::ConstLinearType v = m.linear(); |
158 |
✓✗ | 2 |
const typename MotionDerived::ConstAngularType w = m.angular(); |
159 |
|||
160 |
✓✗ | 2 |
DenseBase res; |
161 |
✓✗✓✗ |
2 |
cross(v,m_S,res.template middleRows<3>(LINEAR)); |
162 |
✓✗✓✗ |
2 |
cross(w,m_S,res.template middleRows<3>(ANGULAR)); |
163 |
|||
164 |
4 |
return res; |
|
165 |
} |
||
166 |
|||
167 |
6207 |
Matrix3 & angularSubspace() { return m_S; } |
|
168 |
7 |
const Matrix3 & angularSubspace() const { return m_S; } |
|
169 |
|||
170 |
17 |
bool isEqual(const ConstraintSphericalZYXTpl & other) const |
|
171 |
{ |
||
172 |
17 |
return m_S == other.m_S; |
|
173 |
} |
||
174 |
|||
175 |
protected: |
||
176 |
Matrix3 m_S; |
||
177 |
|||
178 |
}; // struct ConstraintSphericalZYXTpl |
||
179 |
|||
180 |
/* [CRBA] ForceSet operator* (Inertia Y,Constraint S) */ |
||
181 |
template <typename S1, int O1, typename S2, int O2> |
||
182 |
Eigen::Matrix<S1,6,3,O1> |
||
183 |
5 |
operator*(const InertiaTpl<S1,O1> & Y, |
|
184 |
const ConstraintSphericalZYXTpl<S2,O2> & S) |
||
185 |
{ |
||
186 |
typedef typename InertiaTpl<S1,O1>::Symmetric3 Symmetric3; |
||
187 |
typedef ConstraintSphericalZYXTpl<S2,O2> Constraint; |
||
188 |
✓✗ | 5 |
Eigen::Matrix<S1,6,3,O1> M; |
189 |
✓✗✓✗ |
5 |
alphaSkew (-Y.mass(),Y.lever(),M.template middleRows<3>(Constraint::LINEAR)); |
190 |
✓✗✓✗ ✓✗✓✗ |
5 |
M.template middleRows<3>(Constraint::ANGULAR) = (Y.inertia () - |
191 |
5 |
typename Symmetric3::AlphaSkewSquare(Y.mass (), Y.lever ())).matrix(); |
|
192 |
|||
193 |
✓✗✓✗ |
10 |
return (M * S.angularSubspace()).eval(); |
194 |
} |
||
195 |
|||
196 |
/* [ABA] Y*S operator (Inertia Y,Constraint S) */ |
||
197 |
// inline Eigen::Matrix<double,6,3> |
||
198 |
template<typename Matrix6Like, typename S2, int O2> |
||
199 |
const typename MatrixMatrixProduct< |
||
200 |
typename Eigen::internal::remove_const<typename SizeDepType<3>::ColsReturn<Matrix6Like>::ConstType>::type, |
||
201 |
typename ConstraintSphericalZYXTpl<S2,O2>::Matrix3 |
||
202 |
>::type |
||
203 |
2 |
operator*(const Eigen::MatrixBase<Matrix6Like> & Y, |
|
204 |
const ConstraintSphericalZYXTpl<S2,O2> & S) |
||
205 |
{ |
||
206 |
EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix6Like,6,6); |
||
207 |
✓✗ | 2 |
return Y.derived().template middleCols<3>(Inertia::ANGULAR) * S.angularSubspace(); |
208 |
} |
||
209 |
|||
210 |
template<typename S1, int O1> |
||
211 |
struct SE3GroupAction< ConstraintSphericalZYXTpl<S1,O1> > |
||
212 |
{ |
||
213 |
// typedef const typename Eigen::ProductReturnType< |
||
214 |
// Eigen::Matrix <double,6,3,0>, |
||
215 |
// Eigen::Matrix <double,3,3,0> |
||
216 |
// >::Type Type; |
||
217 |
typedef Eigen::Matrix<S1,6,3,O1> ReturnType; |
||
218 |
}; |
||
219 |
|||
220 |
template<typename S1, int O1, typename MotionDerived> |
||
221 |
struct MotionAlgebraAction< ConstraintSphericalZYXTpl<S1,O1>, MotionDerived > |
||
222 |
{ |
||
223 |
typedef Eigen::Matrix<S1,6,3,O1> ReturnType; |
||
224 |
}; |
||
225 |
|||
226 |
template<typename Scalar, int Options> struct JointSphericalZYXTpl; |
||
227 |
|||
228 |
template<typename _Scalar, int _Options> |
||
229 |
struct traits< JointSphericalZYXTpl<_Scalar,_Options> > |
||
230 |
{ |
||
231 |
enum { |
||
232 |
NQ = 3, |
||
233 |
NV = 3 |
||
234 |
}; |
||
235 |
typedef _Scalar Scalar; |
||
236 |
enum { Options = _Options }; |
||
237 |
typedef JointDataSphericalZYXTpl<Scalar,Options> JointDataDerived; |
||
238 |
typedef JointModelSphericalZYXTpl<Scalar,Options> JointModelDerived; |
||
239 |
typedef ConstraintSphericalZYXTpl<Scalar,Options> Constraint_t; |
||
240 |
typedef SE3Tpl<Scalar,Options> Transformation_t; |
||
241 |
typedef MotionSphericalTpl<Scalar,Options> Motion_t; |
||
242 |
typedef MotionSphericalTpl<Scalar,Options> Bias_t; |
||
243 |
|||
244 |
// [ABA] |
||
245 |
typedef Eigen::Matrix<Scalar,6,NV,Options> U_t; |
||
246 |
typedef Eigen::Matrix<Scalar,NV,NV,Options> D_t; |
||
247 |
typedef Eigen::Matrix<Scalar,6,NV,Options> UD_t; |
||
248 |
|||
249 |
PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE |
||
250 |
|||
251 |
typedef Eigen::Matrix<Scalar,NQ,1,Options> ConfigVector_t; |
||
252 |
typedef Eigen::Matrix<Scalar,NV,1,Options> TangentVector_t; |
||
253 |
}; |
||
254 |
|||
255 |
template<typename Scalar, int Options> |
||
256 |
struct traits< JointDataSphericalZYXTpl<Scalar,Options> > |
||
257 |
{ typedef JointSphericalZYXTpl<Scalar,Options> JointDerived; }; |
||
258 |
|||
259 |
template<typename Scalar, int Options> |
||
260 |
struct traits< JointModelSphericalZYXTpl<Scalar,Options> > |
||
261 |
{ typedef JointSphericalZYXTpl<Scalar,Options> JointDerived; }; |
||
262 |
|||
263 |
template<typename _Scalar, int _Options> |
||
264 |
struct JointDataSphericalZYXTpl : public JointDataBase< JointDataSphericalZYXTpl<_Scalar,_Options> > |
||
265 |
{ |
||
266 |
46 |
EIGEN_MAKE_ALIGNED_OPERATOR_NEW |
|
267 |
typedef JointSphericalZYXTpl<_Scalar,_Options> JointDerived; |
||
268 |
PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(JointDerived); |
||
269 |
670 |
PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR |
|
270 |
|||
271 |
Constraint_t S; |
||
272 |
Transformation_t M; |
||
273 |
Motion_t v; |
||
274 |
Bias_t c; |
||
275 |
|||
276 |
// [ABA] specific data |
||
277 |
U_t U; |
||
278 |
D_t Dinv; |
||
279 |
UD_t UDinv; |
||
280 |
D_t StU; |
||
281 |
|||
282 |
3124 |
JointDataSphericalZYXTpl () |
|
283 |
: S(Constraint_t::Matrix3::Zero()) |
||
284 |
, M(Transformation_t::Identity()) |
||
285 |
, v(Motion_t::Vector3::Zero()) |
||
286 |
, c(Bias_t::Vector3::Zero()) |
||
287 |
, U(U_t::Zero()) |
||
288 |
, Dinv(D_t::Zero()) |
||
289 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
3124 |
, UDinv(UD_t::Zero()) |
290 |
3124 |
{} |
|
291 |
|||
292 |
✓✗ | 44 |
static std::string classname() { return std::string("JointDataSphericalZYX"); } |
293 |
3 |
std::string shortname() const { return classname(); } |
|
294 |
|||
295 |
}; // strcut JointDataSphericalZYXTpl |
||
296 |
|||
297 |
PINOCCHIO_JOINT_CAST_TYPE_SPECIALIZATION(JointModelSphericalZYXTpl); |
||
298 |
template<typename _Scalar, int _Options> |
||
299 |
struct JointModelSphericalZYXTpl |
||
300 |
: public JointModelBase< JointModelSphericalZYXTpl<_Scalar,_Options> > |
||
301 |
{ |
||
302 |
150 |
EIGEN_MAKE_ALIGNED_OPERATOR_NEW |
|
303 |
typedef JointSphericalZYXTpl<_Scalar,_Options> JointDerived; |
||
304 |
PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived); |
||
305 |
|||
306 |
typedef JointModelBase<JointModelSphericalZYXTpl> Base; |
||
307 |
using Base::id; |
||
308 |
using Base::idx_q; |
||
309 |
using Base::idx_v; |
||
310 |
using Base::setIndexes; |
||
311 |
|||
312 |
3098 |
JointDataDerived createData() const { return JointDataDerived(); } |
|
313 |
|||
314 |
const std::vector<bool> hasConfigurationLimit() const |
||
315 |
{ |
||
316 |
return {true, true, true}; |
||
317 |
} |
||
318 |
|||
319 |
const std::vector<bool> hasConfigurationLimitInTangent() const |
||
320 |
{ |
||
321 |
return {true, true, true}; |
||
322 |
} |
||
323 |
|||
324 |
template<typename ConfigVector> |
||
325 |
4102 |
void calc(JointDataDerived & data, |
|
326 |
const typename Eigen::MatrixBase<ConfigVector> & qs) const |
||
327 |
{ |
||
328 |
✓✗✓✗ |
4102 |
typename ConfigVector::template ConstFixedSegmentReturnType<NQ>::Type & q = qs.template segment<NQ>(idx_q()); |
329 |
|||
330 |
typedef typename ConfigVector::Scalar S2; |
||
331 |
|||
332 |
✓✗ | 4102 |
S2 c0,s0; SINCOS(q(0), &s0, &c0); |
333 |
✓✗ | 4102 |
S2 c1,s1; SINCOS(q(1), &s1, &c1); |
334 |
✓✗ | 4102 |
S2 c2,s2; SINCOS(q(2), &s2, &c2); |
335 |
|||
336 |
✓✗✓✗ |
4102 |
data.M.rotation () << c0 * c1, |
337 |
✓✗ | 4102 |
c0 * s1 * s2 - s0 * c2, |
338 |
✓✗ | 4102 |
c0 * s1 * c2 + s0 * s2, |
339 |
✓✗ | 4102 |
s0 * c1, |
340 |
✓✗ | 4102 |
s0 * s1 * s2 + c0 * c2, |
341 |
✓✗✓✗ |
4102 |
s0 * s1 * c2 - c0 * s2, |
342 |
8204 |
-s1, |
|
343 |
✓✗ | 4102 |
c1 * s2, |
344 |
✓✗ | 4102 |
c1 * c2; |
345 |
|||
346 |
data.S.angularSubspace() |
||
347 |
✓✗✓✗ ✓✗ |
4102 |
<< -s1, Scalar(0), Scalar(1), |
348 |
✓✗✓✗ ✓✗ |
4102 |
c1 * s2, c2, Scalar(0), |
349 |
✓✗✓✗ ✓✗ |
4102 |
c1 * c2, -s2, Scalar(0); |
350 |
4102 |
} |
|
351 |
|||
352 |
template<typename ConfigVector, typename TangentVector> |
||
353 |
1040 |
void calc(JointDataDerived & data, |
|
354 |
const typename Eigen::MatrixBase<ConfigVector> & qs, |
||
355 |
const typename Eigen::MatrixBase<TangentVector> & vs) const |
||
356 |
{ |
||
357 |
✓✗✓✗ |
1040 |
typename ConfigVector::template ConstFixedSegmentReturnType<NQ>::Type & q = qs.template segment<NQ>(idx_q()); |
358 |
|||
359 |
typedef typename ConfigVector::Scalar S2; |
||
360 |
|||
361 |
✓✗ | 1040 |
S2 c0,s0; SINCOS(q(0), &s0, &c0); |
362 |
✓✗ | 1040 |
S2 c1,s1; SINCOS(q(1), &s1, &c1); |
363 |
✓✗ | 1040 |
S2 c2,s2; SINCOS(q(2), &s2, &c2); |
364 |
|||
365 |
✓✗✓✗ |
1040 |
data.M.rotation () << c0 * c1, |
366 |
✓✗ | 1040 |
c0 * s1 * s2 - s0 * c2, |
367 |
✓✗ | 1040 |
c0 * s1 * c2 + s0 * s2, |
368 |
✓✗ | 1040 |
s0 * c1, |
369 |
✓✗ | 1040 |
s0 * s1 * s2 + c0 * c2, |
370 |
✓✗✓✗ |
1040 |
s0 * s1 * c2 - c0 * s2, |
371 |
2080 |
-s1, |
|
372 |
✓✗ | 1040 |
c1 * s2, |
373 |
✓✗ | 1040 |
c1 * c2; |
374 |
|||
375 |
data.S.angularSubspace() |
||
376 |
✓✗✓✗ ✓✗ |
1040 |
<< -s1, Scalar(0), Scalar(1), |
377 |
✓✗✓✗ ✓✗ |
1040 |
c1 * s2, c2, Scalar(0), |
378 |
✓✗✓✗ ✓✗ |
1040 |
c1 * c2, -s2, Scalar(0); |
379 |
|||
380 |
typename TangentVector::template ConstFixedSegmentReturnType<NV>::Type |
||
381 |
✓✗✓✗ |
1040 |
& q_dot = vs.template segment<NV>(idx_v()); |
382 |
|||
383 |
✓✗✓✗ ✓✗ |
1040 |
data.v().noalias() = data.S.angularSubspace() * q_dot; |
384 |
|||
385 |
✓✗✓✗ ✓✗ |
1040 |
data.c()(0) = -c1 * q_dot(0) * q_dot(1); |
386 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
1040 |
data.c()(1) = -s1 * s2 * q_dot(0) * q_dot(1) + c1 * c2 * q_dot(0) * q_dot(2) - s2 * q_dot(1) * q_dot(2); |
387 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
1040 |
data.c()(2) = -s1 * c2 * q_dot(0) * q_dot(1) - c1 * s2 * q_dot(0) * q_dot(2) - c2 * q_dot(1) * q_dot(2); |
388 |
1040 |
} |
|
389 |
|||
390 |
template<typename Matrix6Like> |
||
391 |
8 |
void calc_aba(JointDataDerived & data, |
|
392 |
const Eigen::MatrixBase<Matrix6Like> & I, |
||
393 |
const bool update_I) const |
||
394 |
{ |
||
395 |
✓✗✓✗ ✓✗ |
8 |
data.U.noalias() = I.template middleCols<3>(Motion::ANGULAR) * data.S.angularSubspace(); |
396 |
✓✗✓✗ ✓✗✓✗ |
8 |
data.StU.noalias() = data.S.angularSubspace().transpose() * data.U.template middleRows<3>(Motion::ANGULAR); |
397 |
|||
398 |
// compute inverse |
||
399 |
// data.Dinv.setIdentity(); |
||
400 |
// data.StU.llt().solveInPlace(data.Dinv); |
||
401 |
8 |
internal::PerformStYSInversion<Scalar>::run(data.StU,data.Dinv); |
|
402 |
|||
403 |
✓✗✓✗ |
8 |
data.UDinv.noalias() = data.U * data.Dinv; |
404 |
|||
405 |
✓✓ | 8 |
if (update_I) |
406 |
✓✗✓✗ |
3 |
PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like,I) -= data.UDinv * data.U.transpose(); |
407 |
8 |
} |
|
408 |
|||
409 |
✓✗ | 16390 |
static std::string classname() { return std::string("JointModelSphericalZYX"); } |
410 |
16348 |
std::string shortname() const { return classname(); } |
|
411 |
|||
412 |
/// \returns An expression of *this with the Scalar type casted to NewScalar. |
||
413 |
template<typename NewScalar> |
||
414 |
4 |
JointModelSphericalZYXTpl<NewScalar,Options> cast() const |
|
415 |
{ |
||
416 |
typedef JointModelSphericalZYXTpl<NewScalar,Options> ReturnType; |
||
417 |
4 |
ReturnType res; |
|
418 |
4 |
res.setIndexes(id(),idx_q(),idx_v()); |
|
419 |
4 |
return res; |
|
420 |
} |
||
421 |
|||
422 |
}; // struct JointModelSphericalZYXTpl |
||
423 |
|||
424 |
} // namespace pinocchio |
||
425 |
|||
426 |
#include <boost/type_traits.hpp> |
||
427 |
|||
428 |
namespace boost |
||
429 |
{ |
||
430 |
template<typename Scalar, int Options> |
||
431 |
struct has_nothrow_constructor< ::pinocchio::JointModelSphericalZYXTpl<Scalar,Options> > |
||
432 |
: public integral_constant<bool,true> {}; |
||
433 |
|||
434 |
template<typename Scalar, int Options> |
||
435 |
struct has_nothrow_copy< ::pinocchio::JointModelSphericalZYXTpl<Scalar,Options> > |
||
436 |
: public integral_constant<bool,true> {}; |
||
437 |
|||
438 |
template<typename Scalar, int Options> |
||
439 |
struct has_nothrow_constructor< ::pinocchio::JointDataSphericalZYXTpl<Scalar,Options> > |
||
440 |
: public integral_constant<bool,true> {}; |
||
441 |
|||
442 |
template<typename Scalar, int Options> |
||
443 |
struct has_nothrow_copy< ::pinocchio::JointDataSphericalZYXTpl<Scalar,Options> > |
||
444 |
: public integral_constant<bool,true> {}; |
||
445 |
} |
||
446 |
|||
447 |
#endif // ifndef __pinocchio_joint_spherical_ZYX_hpp__ |
Generated by: GCOVR (Version 4.2) |