| GCC Code Coverage Report | |||||||||||||||||||||
|
|||||||||||||||||||||
| Line | Branch | Exec | Source |
1 |
// |
||
2 |
// Copyright (c) 2016-2020 CNRS INRIA |
||
3 |
// |
||
4 |
|||
5 |
#ifndef __pinocchio_multibody_liegroup_special_euclidean_operation_hpp__ |
||
6 |
#define __pinocchio_multibody_liegroup_special_euclidean_operation_hpp__ |
||
7 |
|||
8 |
#include <limits> |
||
9 |
|||
10 |
#include "pinocchio/macros.hpp" |
||
11 |
#include "pinocchio/math/quaternion.hpp" |
||
12 |
#include "pinocchio/spatial/fwd.hpp" |
||
13 |
#include "pinocchio/utils/static-if.hpp" |
||
14 |
#include "pinocchio/spatial/se3.hpp" |
||
15 |
#include "pinocchio/multibody/liegroup/liegroup-base.hpp" |
||
16 |
|||
17 |
#include "pinocchio/multibody/liegroup/vector-space.hpp" |
||
18 |
#include "pinocchio/multibody/liegroup/cartesian-product.hpp" |
||
19 |
#include "pinocchio/multibody/liegroup/special-orthogonal.hpp" |
||
20 |
|||
21 |
namespace pinocchio |
||
22 |
{ |
||
23 |
template<int Dim, typename Scalar, int Options = 0> |
||
24 |
struct SpecialEuclideanOperationTpl |
||
25 |
{}; |
||
26 |
|||
27 |
template<int Dim, typename Scalar, int Options> |
||
28 |
struct traits< SpecialEuclideanOperationTpl<Dim,Scalar,Options> > |
||
29 |
{}; |
||
30 |
|||
31 |
template<typename _Scalar, int _Options> |
||
32 |
struct traits< SpecialEuclideanOperationTpl<2,_Scalar,_Options> > |
||
33 |
{ |
||
34 |
typedef _Scalar Scalar; |
||
35 |
enum |
||
36 |
{ |
||
37 |
Options = _Options, |
||
38 |
NQ = 4, |
||
39 |
NV = 3 |
||
40 |
}; |
||
41 |
}; |
||
42 |
|||
43 |
// SE(2) |
||
44 |
template<typename _Scalar, int _Options> |
||
45 |
struct SpecialEuclideanOperationTpl<2,_Scalar,_Options> |
||
46 |
: public LieGroupBase <SpecialEuclideanOperationTpl<2,_Scalar,_Options> > |
||
47 |
{ |
||
48 |
PINOCCHIO_LIE_GROUP_TPL_PUBLIC_INTERFACE(SpecialEuclideanOperationTpl); |
||
49 |
|||
50 |
typedef VectorSpaceOperationTpl<2,Scalar,Options> R2_t; |
||
51 |
typedef SpecialOrthogonalOperationTpl<2,Scalar,Options> SO2_t; |
||
52 |
typedef CartesianProductOperation<R2_t, SO2_t> R2crossSO2_t; |
||
53 |
|||
54 |
typedef Eigen::Matrix<Scalar,2,2,Options> Matrix2; |
||
55 |
typedef Eigen::Matrix<Scalar,2,1,Options> Vector2; |
||
56 |
|||
57 |
template<typename TangentVector, typename Matrix2Like, typename Vector2Like> |
||
58 |
11383 |
static void exp(const Eigen::MatrixBase<TangentVector> & v, |
|
59 |
const Eigen::MatrixBase<Matrix2Like> & R, |
||
60 |
const Eigen::MatrixBase<Vector2Like> & t) |
||
61 |
{ |
||
62 |
EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(TangentVector_t,TangentVector); |
||
63 |
EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix2Like, 2, 2); |
||
64 |
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector2Like, 2); |
||
65 |
|||
66 |
typedef typename Matrix2Like::Scalar Scalar; |
||
67 |
✓✗ | 11383 |
const Scalar omega = v(2); |
68 |
11383 |
Scalar cv,sv; SINCOS(omega, &sv, &cv); |
|
69 |
✓✗✓✗ ✓✗✓✗ |
11383 |
PINOCCHIO_EIGEN_CONST_CAST(Matrix2Like,R) << cv, -sv, sv, cv; |
70 |
using internal::if_then_else; |
||
71 |
|||
72 |
{ |
||
73 |
✓✗✓✗ ✓✗ |
11383 |
typename PINOCCHIO_EIGEN_PLAIN_TYPE(Vector2Like) vcross(-v(1), v(0)); |
74 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
11383 |
vcross -= -v(1)*R.col(0) + v(0)*R.col(1); |
75 |
✓✗ | 11383 |
vcross /= omega; |
76 |
11383 |
Scalar omega_abs = math::fabs(omega); |
|
77 |
✓✗✓✗ ✓✓✓✗ ✓✗ |
11383 |
PINOCCHIO_EIGEN_CONST_CAST(Vector2Like,t).coeffRef(0) = if_then_else(internal::GT, omega_abs , Scalar(1e-14), |
78 |
vcross.coeff(0), |
||
79 |
v.coeff(0)); |
||
80 |
|||
81 |
✓✗✓✗ ✓✓✓✗ ✓✗ |
11383 |
PINOCCHIO_EIGEN_CONST_CAST(Vector2Like,t).coeffRef(1) = if_then_else(internal::GT, omega_abs, Scalar(1e-14), |
82 |
vcross.coeff(1), |
||
83 |
v.coeff(1)); |
||
84 |
} |
||
85 |
|||
86 |
11383 |
} |
|
87 |
|||
88 |
template<typename Matrix2Like, typename Vector2Like, typename Matrix3Like> |
||
89 |
36 |
static void toInverseActionMatrix(const Eigen::MatrixBase<Matrix2Like> & R, |
|
90 |
const Eigen::MatrixBase<Vector2Like> & t, |
||
91 |
const Eigen::MatrixBase<Matrix3Like> & M, |
||
92 |
const AssignmentOperatorType op) |
||
93 |
{ |
||
94 |
EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix2Like, 2, 2); |
||
95 |
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector2Like, 2); |
||
96 |
✓✗✓✓ ✗✓✗✓ ✗ |
36 |
PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix3Like, M, 3, 3); |
97 |
36 |
Matrix3Like & Mout = PINOCCHIO_EIGEN_CONST_CAST(Matrix3Like,M); |
|
98 |
typedef typename Matrix3Like::Scalar Scalar; |
||
99 |
|||
100 |
✓✗✓✗ ✓✗✓✗ |
36 |
typename PINOCCHIO_EIGEN_PLAIN_TYPE(Vector2Like) tinv((R.transpose() * t).reverse()); |
101 |
✓✗ | 36 |
tinv[0] *= Scalar(-1.); |
102 |
✓✓✓✗ |
36 |
switch(op) |
103 |
{ |
||
104 |
34 |
case SETTO: |
|
105 |
✓✗✓✗ ✓✗ |
34 |
Mout.template topLeftCorner<2,2>() = R.transpose(); |
106 |
✓✗✓✗ |
34 |
Mout.template topRightCorner<2,1>() = tinv; |
107 |
✓✗✓✗ |
34 |
Mout.template bottomLeftCorner<1,2>().setZero(); |
108 |
✓✗ | 34 |
Mout(2,2) = (Scalar)1; |
109 |
34 |
break; |
|
110 |
1 |
case ADDTO: |
|
111 |
✓✗✓✗ ✓✗ |
1 |
Mout.template topLeftCorner<2,2>() += R.transpose(); |
112 |
✓✗✓✗ |
1 |
Mout.template topRightCorner<2,1>() += tinv; |
113 |
✓✗ | 1 |
Mout(2,2) += (Scalar)1; |
114 |
1 |
break; |
|
115 |
1 |
case RMTO: |
|
116 |
✓✗✓✗ ✓✗ |
1 |
Mout.template topLeftCorner<2,2>() -= R.transpose(); |
117 |
✓✗✓✗ |
1 |
Mout.template topRightCorner<2,1>() -= tinv; |
118 |
✓✗ | 1 |
Mout(2,2) -= (Scalar)1; |
119 |
1 |
break; |
|
120 |
default: |
||
121 |
assert(false && "Wrong Op requesed value"); |
||
122 |
break; |
||
123 |
} |
||
124 |
|||
125 |
|||
126 |
|||
127 |
36 |
} |
|
128 |
|||
129 |
template<typename Matrix2Like, typename Vector2Like, typename TangentVector> |
||
130 |
10987 |
static void log(const Eigen::MatrixBase<Matrix2Like> & R, |
|
131 |
const Eigen::MatrixBase<Vector2Like> & p, |
||
132 |
const Eigen::MatrixBase<TangentVector> & v) |
||
133 |
{ |
||
134 |
EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix2Like, 2, 2); |
||
135 |
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector2Like, 2); |
||
136 |
EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(TangentVector_t,TangentVector); |
||
137 |
|||
138 |
10987 |
TangentVector & vout = PINOCCHIO_EIGEN_CONST_CAST(TangentVector,v); |
|
139 |
|||
140 |
typedef typename Matrix2Like::Scalar Scalar1; |
||
141 |
|||
142 |
✓✗ | 10987 |
Scalar1 t = SO2_t::log(R); |
143 |
10987 |
const Scalar1 tabs = math::fabs(t); |
|
144 |
10987 |
const Scalar1 t2 = t*t; |
|
145 |
10987 |
Scalar1 st,ct; SINCOS(tabs, &st, &ct); |
|
146 |
Scalar1 alpha; |
||
147 |
alpha = internal::if_then_else(internal::LT, tabs, Scalar(1e-4), |
||
148 |
1 - t2/12 - t2*t2/720, |
||
149 |
✓✗ | 10987 |
tabs*st/(2*(1-ct))); |
150 |
|||
151 |
✓✗✓✗ ✓✗✓✗ |
10987 |
vout.template head<2>().noalias() = alpha * p; |
152 |
✓✗✓✗ |
10987 |
vout(0) += t/2 * p(1); |
153 |
✓✗✓✗ |
10987 |
vout(1) += -t/2 * p(0); |
154 |
✓✗ | 10987 |
vout(2) = t; |
155 |
10987 |
} |
|
156 |
|||
157 |
template<typename Matrix2Like, typename Vector2Like, typename JacobianOutLike> |
||
158 |
80 |
static void Jlog(const Eigen::MatrixBase<Matrix2Like> & R, |
|
159 |
const Eigen::MatrixBase<Vector2Like> & p, |
||
160 |
const Eigen::MatrixBase<JacobianOutLike> & J) |
||
161 |
{ |
||
162 |
EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix2Like, 2, 2); |
||
163 |
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector2Like, 2); |
||
164 |
EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(JacobianOutLike, JacobianMatrix_t); |
||
165 |
|||
166 |
80 |
JacobianOutLike & Jout = PINOCCHIO_EIGEN_CONST_CAST(JacobianOutLike,J); |
|
167 |
|||
168 |
typedef typename Matrix2Like::Scalar Scalar1; |
||
169 |
|||
170 |
✓✗ | 80 |
Scalar1 t = SO2_t::log(R); |
171 |
80 |
const Scalar1 tabs = math::fabs(t); |
|
172 |
Scalar1 alpha, alpha_dot; |
||
173 |
80 |
Scalar1 t2 = t*t; |
|
174 |
80 |
Scalar1 st,ct; SINCOS(t, &st, &ct); |
|
175 |
80 |
Scalar1 inv_2_1_ct = 0.5 / (1-ct); |
|
176 |
|||
177 |
alpha = internal::if_then_else(internal::LT, tabs, Scalar(1e-4), |
||
178 |
1 - t2/12, |
||
179 |
✓✗ | 80 |
t*st*inv_2_1_ct); |
180 |
alpha_dot = internal::if_then_else(internal::LT, tabs, Scalar(1e-4), |
||
181 |
- t / 6 - t2*t / 180, |
||
182 |
✓✗ | 80 |
(st-t) * inv_2_1_ct); |
183 |
|||
184 |
✓✗ | 80 |
typename PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix2Like) V; |
185 |
✓✗✓✗ |
80 |
V(0,0) = V(1,1) = alpha; |
186 |
✓✗ | 80 |
V(1,0) = - t / 2; |
187 |
✓✗✓✗ |
80 |
V(0,1) = - V(1,0); |
188 |
|||
189 |
✓✗✓✗ ✓✗✓✗ |
80 |
Jout.template topLeftCorner <2,2>().noalias() = V * R; |
190 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
80 |
Jout.template topRightCorner<2,1>() << alpha_dot*p[0] + p[1]/2, -p(0)/2 + alpha_dot*p(1); |
191 |
✓✗✓✗ |
80 |
Jout.template bottomLeftCorner<1,2>().setZero(); |
192 |
✓✗ | 80 |
Jout(2,2) = 1; |
193 |
80 |
} |
|
194 |
|||
195 |
/// Get dimension of Lie Group vector representation |
||
196 |
/// |
||
197 |
/// For instance, for SO(3), the dimension of the vector representation is |
||
198 |
/// 4 (quaternion) while the dimension of the tangent space is 3. |
||
199 |
9691 |
static Index nq() |
|
200 |
{ |
||
201 |
9691 |
return NQ; |
|
202 |
} |
||
203 |
/// Get dimension of Lie Group tangent space |
||
204 |
5443 |
static Index nv() |
|
205 |
{ |
||
206 |
5443 |
return NV; |
|
207 |
} |
||
208 |
|||
209 |
9 |
static ConfigVector_t neutral() |
|
210 |
{ |
||
211 |
✓✗✓✗ ✓✗✓✗ |
9 |
ConfigVector_t n; n << Scalar(0), Scalar(0), Scalar(1), Scalar(0); |
212 |
9 |
return n; |
|
213 |
} |
||
214 |
|||
215 |
65 |
static std::string name() |
|
216 |
{ |
||
217 |
✓✗ | 65 |
return std::string("SE(2)"); |
218 |
} |
||
219 |
|||
220 |
template <class ConfigL_t, class ConfigR_t, class Tangent_t> |
||
221 |
10987 |
static void difference_impl(const Eigen::MatrixBase<ConfigL_t> & q0, |
|
222 |
const Eigen::MatrixBase<ConfigR_t> & q1, |
||
223 |
const Eigen::MatrixBase<Tangent_t> & d) |
||
224 |
{ |
||
225 |
PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH |
||
226 |
PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED |
||
227 |
✓✗✓✗ ✓✗✓✗ |
10987 |
Matrix2 R0, R1; Vector2 t0, t1; |
228 |
✓✗ | 10987 |
forwardKinematics(R0, t0, q0); |
229 |
✓✗ | 10987 |
forwardKinematics(R1, t1, q1); |
230 |
PINOCCHIO_COMPILER_DIAGNOSTIC_POP |
||
231 |
✓✗✓✗ ✓✗ |
10987 |
Matrix2 R (R0.transpose() * R1); |
232 |
✓✗✓✗ ✓✗✓✗ |
10987 |
Vector2 t (R0.transpose() * (t1 - t0)); |
233 |
|||
234 |
✓✗ | 10987 |
log(R, t, d); |
235 |
10987 |
} |
|
236 |
|||
237 |
template <ArgumentPosition arg, class ConfigL_t, class ConfigR_t, class JacobianOut_t> |
||
238 |
140 |
void dDifference_impl(const Eigen::MatrixBase<ConfigL_t> & q0, |
|
239 |
const Eigen::MatrixBase<ConfigR_t> & q1, |
||
240 |
const Eigen::MatrixBase<JacobianOut_t>& J) const |
||
241 |
{ |
||
242 |
PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH |
||
243 |
PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED |
||
244 |
✓✗✓✗ ✓✗✓✗ |
140 |
Matrix2 R0, R1; Vector2 t0, t1; |
245 |
✓✗ | 140 |
forwardKinematics(R0, t0, q0); |
246 |
✓✗ | 140 |
forwardKinematics(R1, t1, q1); |
247 |
PINOCCHIO_COMPILER_DIAGNOSTIC_POP |
||
248 |
✓✗✓✗ ✓✗ |
140 |
Matrix2 R (R0.transpose() * R1); |
249 |
✓✗✓✗ ✓✗✓✗ |
140 |
Vector2 t (R0.transpose() * (t1 - t0)); |
250 |
|||
251 |
if (arg == ARG0) { |
||
252 |
PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH |
||
253 |
PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED |
||
254 |
✓✗ | 50 |
JacobianMatrix_t J1; |
255 |
✓✗ | 50 |
Jlog (R, t, J1); |
256 |
PINOCCHIO_COMPILER_DIAGNOSTIC_POP |
||
257 |
|||
258 |
// pcross = [ y1-y0, - (x1 - x0) ] |
||
259 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
50 |
Vector2 pcross (q1(1) - q0(1), q0(0) - q1(0)); |
260 |
|||
261 |
50 |
JacobianOut_t& J0 = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, J); |
|
262 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
50 |
J0.template topLeftCorner <2,2> ().noalias() = - R.transpose(); |
263 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
50 |
J0.template topRightCorner<2,1> ().noalias() = R1.transpose() * pcross; |
264 |
✓✗✓✗ |
50 |
J0.template bottomLeftCorner <1,2> ().setZero(); |
265 |
✓✗ | 50 |
J0 (2,2) = -1; |
266 |
✓✗ | 50 |
J0.applyOnTheLeft(J1); |
267 |
} else if (arg == ARG1) { |
||
268 |
✓✗ | 90 |
Jlog (R, t, J); |
269 |
} |
||
270 |
140 |
} |
|
271 |
|||
272 |
template <class ConfigIn_t, class Velocity_t, class ConfigOut_t> |
||
273 |
11311 |
static void integrate_impl(const Eigen::MatrixBase<ConfigIn_t> & q, |
|
274 |
const Eigen::MatrixBase<Velocity_t> & v, |
||
275 |
const Eigen::MatrixBase<ConfigOut_t> & qout) |
||
276 |
{ |
||
277 |
11311 |
ConfigOut_t & out = PINOCCHIO_EIGEN_CONST_CAST(ConfigOut_t,qout); |
|
278 |
|||
279 |
PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH |
||
280 |
PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED |
||
281 |
✓✗✓✗ |
11311 |
Matrix2 R0, R; |
282 |
✓✗✓✗ |
11311 |
Vector2 t0, t; |
283 |
✓✗ | 11311 |
forwardKinematics(R0, t0, q); |
284 |
✓✗ | 11311 |
exp(v, R, t); |
285 |
PINOCCHIO_COMPILER_DIAGNOSTIC_POP |
||
286 |
|||
287 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
11311 |
out.template head<2>().noalias() = R0 * t + t0; |
288 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
11311 |
out.template tail<2>().noalias() = R0 * R.col(0); |
289 |
11311 |
} |
|
290 |
|||
291 |
template <class Config_t, class Jacobian_t> |
||
292 |
20 |
static void integrateCoeffWiseJacobian_impl(const Eigen::MatrixBase<Config_t> & q, |
|
293 |
const Eigen::MatrixBase<Jacobian_t> & J) |
||
294 |
{ |
||
295 |
✓✗✓✗ |
20 |
assert(J.rows() == nq() && J.cols() == nv() && "J is not of the right dimension"); |
296 |
|||
297 |
20 |
Jacobian_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t,J); |
|
298 |
20 |
Jout.setZero(); |
|
299 |
|||
300 |
20 |
const typename Config_t::Scalar & c_theta = q(2), |
|
301 |
20 |
& s_theta = q(3); |
|
302 |
|||
303 |
✓✗✓✗ ✓✗✓✗ |
20 |
Jout.template topLeftCorner<2,2>() << c_theta, -s_theta, s_theta, c_theta; |
304 |
✓✗✓✗ |
20 |
Jout.template bottomRightCorner<2,1>() << -s_theta, c_theta; |
305 |
20 |
} |
|
306 |
|||
307 |
template <class Config_t, class Tangent_t, class JacobianOut_t> |
||
308 |
36 |
static void dIntegrate_dq_impl(const Eigen::MatrixBase<Config_t > & /*q*/, |
|
309 |
const Eigen::MatrixBase<Tangent_t> & v, |
||
310 |
const Eigen::MatrixBase<JacobianOut_t>& J, |
||
311 |
const AssignmentOperatorType op=SETTO) |
||
312 |
{ |
||
313 |
36 |
JacobianOut_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J); |
|
314 |
|||
315 |
PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH |
||
316 |
PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED |
||
317 |
✓✗ | 36 |
Matrix2 R; |
318 |
✓✗ | 36 |
Vector2 t; |
319 |
✓✗ | 36 |
exp(v, R, t); |
320 |
PINOCCHIO_COMPILER_DIAGNOSTIC_POP |
||
321 |
|||
322 |
✓✗ | 36 |
toInverseActionMatrix(R, t, Jout, op); |
323 |
36 |
} |
|
324 |
|||
325 |
template <class Config_t, class Tangent_t, class JacobianOut_t> |
||
326 |
56 |
static void dIntegrate_dv_impl(const Eigen::MatrixBase<Config_t > & /*q*/, |
|
327 |
const Eigen::MatrixBase<Tangent_t> & v, |
||
328 |
const Eigen::MatrixBase<JacobianOut_t> & J, |
||
329 |
const AssignmentOperatorType op=SETTO) |
||
330 |
{ |
||
331 |
56 |
JacobianOut_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J); |
|
332 |
// TODO sparse version |
||
333 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
56 |
MotionTpl<Scalar,0> nu; nu.toVector() << v.template head<2>(), 0, 0, 0, v[2]; |
334 |
PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH |
||
335 |
PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED |
||
336 |
✓✗ | 56 |
Eigen::Matrix<Scalar,6,6> Jtmp6; |
337 |
✓✗ | 56 |
Jexp6(nu, Jtmp6); |
338 |
PINOCCHIO_COMPILER_DIAGNOSTIC_POP |
||
339 |
|||
340 |
✓✓✓✗ |
56 |
switch(op) |
341 |
{ |
||
342 |
54 |
case SETTO: |
|
343 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
54 |
Jout << Jtmp6.template topLeftCorner<2,2>(), Jtmp6.template topRightCorner<2,1>(), |
344 |
✓✗✓✗ |
54 |
Jtmp6.template bottomLeftCorner<1,2>(), Jtmp6.template bottomRightCorner<1,1>(); |
345 |
54 |
break; |
|
346 |
1 |
case ADDTO: |
|
347 |
✓✗✓✗ ✓✗ |
1 |
Jout.template topLeftCorner<2,2>() += Jtmp6.template topLeftCorner<2,2>(); |
348 |
✓✗✓✗ ✓✗ |
1 |
Jout.template topRightCorner<2,1>() += Jtmp6.template topRightCorner<2,1>(); |
349 |
✓✗✓✗ ✓✗ |
1 |
Jout.template bottomLeftCorner<1,2>() += Jtmp6.template bottomLeftCorner<1,2>(); |
350 |
✓✗✓✗ ✓✗ |
1 |
Jout.template bottomRightCorner<1,1>() += Jtmp6.template bottomRightCorner<1,1>(); |
351 |
1 |
break; |
|
352 |
1 |
case RMTO: |
|
353 |
✓✗✓✗ ✓✗ |
1 |
Jout.template topLeftCorner<2,2>() -= Jtmp6.template topLeftCorner<2,2>(); |
354 |
✓✗✓✗ ✓✗ |
1 |
Jout.template topRightCorner<2,1>() -= Jtmp6.template topRightCorner<2,1>(); |
355 |
✓✗✓✗ ✓✗ |
1 |
Jout.template bottomLeftCorner<1,2>() -= Jtmp6.template bottomLeftCorner<1,2>(); |
356 |
✓✗✓✗ ✓✗ |
1 |
Jout.template bottomRightCorner<1,1>() -= Jtmp6.template bottomRightCorner<1,1>(); |
357 |
1 |
break; |
|
358 |
default: |
||
359 |
assert(false && "Wrong Op requesed value"); |
||
360 |
break; |
||
361 |
} |
||
362 |
56 |
} |
|
363 |
|||
364 |
template <class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t> |
||
365 |
1 |
static void dIntegrateTransport_dq_impl(const Eigen::MatrixBase<Config_t > & /*q*/, |
|
366 |
const Eigen::MatrixBase<Tangent_t> & v, |
||
367 |
const Eigen::MatrixBase<JacobianIn_t> & Jin, |
||
368 |
const Eigen::MatrixBase<JacobianOut_t> & J_out) |
||
369 |
{ |
||
370 |
1 |
JacobianOut_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J_out); |
|
371 |
PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH |
||
372 |
PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED |
||
373 |
✓✗ | 1 |
Matrix2 R; |
374 |
✓✗ | 1 |
Vector2 t; |
375 |
✓✗ | 1 |
exp(v, R, t); |
376 |
PINOCCHIO_COMPILER_DIAGNOSTIC_POP |
||
377 |
|||
378 |
✓✗✓✗ ✓✗✓✗ |
1 |
Vector2 tinv = (R.transpose() * t).reverse(); |
379 |
✓✗ | 1 |
tinv[0] *= Scalar(-1.); |
380 |
|||
381 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
1 |
Jout.template topRows<2>().noalias() = R.transpose() * Jin.template topRows<2>(); |
382 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
1 |
Jout.template topRows<2>().noalias() += tinv * Jin.template bottomRows<1>(); |
383 |
✓✗✓✗ ✓✗ |
1 |
Jout.template bottomRows<1>() = Jin.template bottomRows<1>(); |
384 |
1 |
} |
|
385 |
|||
386 |
template <class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t> |
||
387 |
1 |
static void dIntegrateTransport_dv_impl(const Eigen::MatrixBase<Config_t > & /*q*/, |
|
388 |
const Eigen::MatrixBase<Tangent_t> & v, |
||
389 |
const Eigen::MatrixBase<JacobianIn_t> & Jin, |
||
390 |
const Eigen::MatrixBase<JacobianOut_t> & J_out) |
||
391 |
{ |
||
392 |
1 |
JacobianOut_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J_out); |
|
393 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
1 |
MotionTpl<Scalar,0> nu; nu.toVector() << v.template head<2>(), 0, 0, 0, v[2]; |
394 |
|||
395 |
PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH |
||
396 |
PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED |
||
397 |
✓✗ | 1 |
Eigen::Matrix<Scalar,6,6> Jtmp6; |
398 |
✓✗ | 1 |
Jexp6(nu, Jtmp6); |
399 |
PINOCCHIO_COMPILER_DIAGNOSTIC_POP |
||
400 |
|||
401 |
Jout.template topRows<2>().noalias() |
||
402 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
1 |
= Jtmp6.template topLeftCorner<2,2>() * Jin.template topRows<2>(); |
403 |
Jout.template topRows<2>().noalias() |
||
404 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
1 |
+= Jtmp6.template topRightCorner<2,1>() * Jin.template bottomRows<1>(); |
405 |
Jout.template bottomRows<1>().noalias() |
||
406 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
1 |
= Jtmp6.template bottomLeftCorner<1,2>()* Jin.template topRows<2>(); |
407 |
Jout.template bottomRows<1>().noalias() |
||
408 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
1 |
+= Jtmp6.template bottomRightCorner<1,1>() * Jin.template bottomRows<1>(); |
409 |
|||
410 |
1 |
} |
|
411 |
|||
412 |
template <class Config_t, class Tangent_t, class Jacobian_t> |
||
413 |
1 |
static void dIntegrateTransport_dq_impl(const Eigen::MatrixBase<Config_t > & /*q*/, |
|
414 |
const Eigen::MatrixBase<Tangent_t> & v, |
||
415 |
const Eigen::MatrixBase<Jacobian_t> & J) |
||
416 |
{ |
||
417 |
1 |
Jacobian_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t,J); |
|
418 |
PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH |
||
419 |
PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED |
||
420 |
✓✗ | 1 |
Matrix2 R; |
421 |
✓✗ | 1 |
Vector2 t; |
422 |
✓✗ | 1 |
exp(v, R, t); |
423 |
PINOCCHIO_COMPILER_DIAGNOSTIC_POP |
||
424 |
|||
425 |
✓✗✓✗ ✓✗✓✗ |
1 |
Vector2 tinv = (R.transpose() * t).reverse(); |
426 |
✓✗ | 1 |
tinv[0] *= Scalar(-1); |
427 |
//TODO: Aliasing |
||
428 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
1 |
Jout.template topRows<2>() = R.transpose() * Jout.template topRows<2>(); |
429 |
//No Aliasing |
||
430 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
1 |
Jout.template topRows<2>().noalias() += tinv * Jout.template bottomRows<1>(); |
431 |
1 |
} |
|
432 |
|||
433 |
template <class Config_t, class Tangent_t, class Jacobian_t> |
||
434 |
1 |
static void dIntegrateTransport_dv_impl(const Eigen::MatrixBase<Config_t > & /*q*/, |
|
435 |
const Eigen::MatrixBase<Tangent_t> & v, |
||
436 |
const Eigen::MatrixBase<Jacobian_t> & J) |
||
437 |
{ |
||
438 |
1 |
Jacobian_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t,J); |
|
439 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
1 |
MotionTpl<Scalar,0> nu; nu.toVector() << v.template head<2>(), 0, 0, 0, v[2]; |
440 |
|||
441 |
PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH |
||
442 |
PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED |
||
443 |
✓✗ | 1 |
Eigen::Matrix<Scalar,6,6> Jtmp6; |
444 |
✓✗ | 1 |
Jexp6(nu, Jtmp6); |
445 |
PINOCCHIO_COMPILER_DIAGNOSTIC_POP |
||
446 |
//TODO: Remove aliasing |
||
447 |
Jout.template topRows<2>() |
||
448 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
1 |
= Jtmp6.template topLeftCorner<2,2>() * Jout.template topRows<2>(); |
449 |
Jout.template topRows<2>().noalias() |
||
450 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
1 |
+= Jtmp6.template topRightCorner<2,1>() * Jout.template bottomRows<1>(); |
451 |
Jout.template bottomRows<1>() |
||
452 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
1 |
= Jtmp6.template bottomRightCorner<1,1>() * Jout.template bottomRows<1>(); |
453 |
Jout.template bottomRows<1>().noalias() |
||
454 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
1 |
+= Jtmp6.template bottomLeftCorner<1,2>() * Jout.template topRows<2>(); |
455 |
1 |
} |
|
456 |
|||
457 |
template <class Config_t> |
||
458 |
2054 |
static void normalize_impl(const Eigen::MatrixBase<Config_t> & qout) |
|
459 |
{ |
||
460 |
✓✗ | 2054 |
PINOCCHIO_EIGEN_CONST_CAST(Config_t,qout).template tail<2>().normalize(); |
461 |
2054 |
} |
|
462 |
|||
463 |
template <class Config_t> |
||
464 |
10209 |
static bool isNormalized_impl(const Eigen::MatrixBase<Config_t> & qin, |
|
465 |
const Scalar& prec) |
||
466 |
{ |
||
467 |
✓✗ | 10209 |
const Scalar norm = Scalar(qin.template tail<2>().norm()); |
468 |
using std::abs; |
||
469 |
10209 |
return abs(norm - Scalar(1.0)) < prec; |
|
470 |
} |
||
471 |
|||
472 |
template <class Config_t> |
||
473 |
217 |
static void random_impl(const Eigen::MatrixBase<Config_t> & qout) |
|
474 |
{ |
||
475 |
✓✗ | 217 |
R2crossSO2_t().random(qout); |
476 |
217 |
} |
|
477 |
|||
478 |
template <class ConfigL_t, class ConfigR_t, class ConfigOut_t> |
||
479 |
6151 |
static void randomConfiguration_impl(const Eigen::MatrixBase<ConfigL_t> & lower, |
|
480 |
const Eigen::MatrixBase<ConfigR_t> & upper, |
||
481 |
const Eigen::MatrixBase<ConfigOut_t> & qout) |
||
482 |
{ |
||
483 |
✓✗ | 6151 |
R2crossSO2_t ().randomConfiguration(lower, upper, qout); |
484 |
6151 |
} |
|
485 |
|||
486 |
template <class ConfigL_t, class ConfigR_t> |
||
487 |
5 |
static bool isSameConfiguration_impl(const Eigen::MatrixBase<ConfigL_t> & q0, |
|
488 |
const Eigen::MatrixBase<ConfigR_t> & q1, |
||
489 |
const Scalar & prec) |
||
490 |
{ |
||
491 |
✓✗ | 5 |
return R2crossSO2_t().isSameConfiguration(q0, q1, prec); |
492 |
} |
||
493 |
|||
494 |
protected: |
||
495 |
|||
496 |
template<typename Matrix2Like, typename Vector2Like, typename Vector4Like> |
||
497 |
33179 |
static void forwardKinematics(const Eigen::MatrixBase<Matrix2Like> & R, |
|
498 |
const Eigen::MatrixBase<Vector2Like> & t, |
||
499 |
const Eigen::MatrixBase<Vector4Like> & q) |
||
500 |
{ |
||
501 |
EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(Matrix2Like, Matrix2); |
||
502 |
EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(Vector2Like, Vector2); |
||
503 |
EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigVector_t,Vector4Like); |
||
504 |
|||
505 |
✓✗ | 33179 |
PINOCCHIO_EIGEN_CONST_CAST(Vector2Like,t) = q.template head<2>(); |
506 |
33179 |
const typename Vector4Like::Scalar & c_theta = q(2), |
|
507 |
33179 |
& s_theta = q(3); |
|
508 |
✓✗✓✗ ✓✗ |
33179 |
PINOCCHIO_EIGEN_CONST_CAST(Matrix2Like,R) << c_theta, -s_theta, s_theta, c_theta; |
509 |
|||
510 |
33179 |
} |
|
511 |
}; // struct SpecialEuclideanOperationTpl<2> |
||
512 |
|||
513 |
template<typename _Scalar, int _Options> |
||
514 |
struct traits< SpecialEuclideanOperationTpl<3,_Scalar,_Options> > |
||
515 |
{ |
||
516 |
typedef _Scalar Scalar; |
||
517 |
enum |
||
518 |
{ |
||
519 |
Options = _Options, |
||
520 |
NQ = 7, |
||
521 |
NV = 6 |
||
522 |
}; |
||
523 |
}; |
||
524 |
|||
525 |
/// SE(3) |
||
526 |
template<typename _Scalar, int _Options> |
||
527 |
struct SpecialEuclideanOperationTpl<3,_Scalar,_Options> |
||
528 |
: public LieGroupBase <SpecialEuclideanOperationTpl<3,_Scalar,_Options> > |
||
529 |
{ |
||
530 |
PINOCCHIO_LIE_GROUP_TPL_PUBLIC_INTERFACE(SpecialEuclideanOperationTpl); |
||
531 |
|||
532 |
typedef CartesianProductOperation <VectorSpaceOperationTpl<3,Scalar,Options>, SpecialOrthogonalOperationTpl<3,Scalar,Options> > R3crossSO3_t; |
||
533 |
|||
534 |
typedef Eigen::Quaternion<Scalar,Options> Quaternion_t; |
||
535 |
typedef Eigen::Map< Quaternion_t> QuaternionMap_t; |
||
536 |
typedef Eigen::Map<const Quaternion_t> ConstQuaternionMap_t; |
||
537 |
typedef SE3Tpl<Scalar,Options> Transformation_t; |
||
538 |
typedef SE3Tpl<Scalar,Options> SE3; |
||
539 |
typedef typename Eigen::NumTraits<Scalar>::Real RealScalar; |
||
540 |
|||
541 |
/// Get dimension of Lie Group vector representation |
||
542 |
/// |
||
543 |
/// For instance, for SO(3), the dimension of the vector representation is |
||
544 |
/// 4 (quaternion) while the dimension of the tangent space is 3. |
||
545 |
10056 |
static Index nq() |
|
546 |
{ |
||
547 |
10056 |
return NQ; |
|
548 |
} |
||
549 |
/// Get dimension of Lie Group tangent space |
||
550 |
4666 |
static Index nv() |
|
551 |
{ |
||
552 |
4666 |
return NV; |
|
553 |
} |
||
554 |
|||
555 |
26 |
static ConfigVector_t neutral() |
|
556 |
{ |
||
557 |
✓✗ | 26 |
ConfigVector_t n; n.template head<6>().setZero(); n[6] = 1; |
558 |
26 |
return n; |
|
559 |
} |
||
560 |
|||
561 |
67 |
static std::string name() |
|
562 |
{ |
||
563 |
✓✗ | 67 |
return std::string("SE(3)"); |
564 |
} |
||
565 |
|||
566 |
template <class ConfigL_t, class ConfigR_t, class Tangent_t> |
||
567 |
11532 |
static void difference_impl(const Eigen::MatrixBase<ConfigL_t> & q0, |
|
568 |
const Eigen::MatrixBase<ConfigR_t> & q1, |
||
569 |
const Eigen::MatrixBase<Tangent_t> & d) |
||
570 |
{ |
||
571 |
✓✗✓✗ |
11532 |
ConstQuaternionMap_t quat0 (q0.derived().template tail<4>().data()); |
572 |
✓✗✗✓ |
11532 |
assert(quaternion::isNormalized(quat0,RealScalar(PINOCCHIO_DEFAULT_QUATERNION_NORM_TOLERANCE_VALUE))); |
573 |
✓✗✓✗ |
11532 |
ConstQuaternionMap_t quat1 (q1.derived().template tail<4>().data()); |
574 |
✓✗✗✓ |
11532 |
assert(quaternion::isNormalized(quat1,RealScalar(PINOCCHIO_DEFAULT_QUATERNION_NORM_TOLERANCE_VALUE))); |
575 |
|||
576 |
11532 |
PINOCCHIO_EIGEN_CONST_CAST(Tangent_t,d) |
|
577 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
11532 |
= log6( SE3(quat0.matrix(), q0.derived().template head<3>()).inverse() |
578 |
11532 |
* SE3(quat1.matrix(), q1.derived().template head<3>())).toVector(); |
|
579 |
11532 |
} |
|
580 |
|||
581 |
/// \cheatsheet \f$ \frac{\partial\ominus}{\partial q_1} {}^1X_0 = - \frac{\partial\ominus}{\partial q_0} \f$ |
||
582 |
template <ArgumentPosition arg, class ConfigL_t, class ConfigR_t, class JacobianOut_t> |
||
583 |
228 |
static void dDifference_impl (const Eigen::MatrixBase<ConfigL_t> & q0, |
|
584 |
const Eigen::MatrixBase<ConfigR_t> & q1, |
||
585 |
const Eigen::MatrixBase<JacobianOut_t> & J) |
||
586 |
{ |
||
587 |
typedef typename SE3::Vector3 Vector3; |
||
588 |
typedef typename SE3::Matrix3 Matrix3; |
||
589 |
|||
590 |
✓✗✓✗ |
228 |
ConstQuaternionMap_t quat0 (q0.derived().template tail<4>().data()); |
591 |
✓✗✗✓ |
228 |
assert(quaternion::isNormalized(quat0,RealScalar(PINOCCHIO_DEFAULT_QUATERNION_NORM_TOLERANCE_VALUE))); |
592 |
✓✗✓✗ |
228 |
ConstQuaternionMap_t quat1 (q1.derived().template tail<4>().data()); |
593 |
✓✗✗✓ |
228 |
assert(quaternion::isNormalized(quat1,RealScalar(PINOCCHIO_DEFAULT_QUATERNION_NORM_TOLERANCE_VALUE))); |
594 |
|||
595 |
✓✗✓✗ |
228 |
Matrix3 R0(quat0.matrix()), R1 (quat1.matrix()); |
596 |
✓✗✗✓ ✓✗✗✓ |
228 |
assert(isUnitary(R0)); assert(isUnitary(R1)); |
597 |
|||
598 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
228 |
const SE3 M ( SE3(R0, q0.template head<3>()).inverse() |
599 |
* SE3(R1, q1.template head<3>())); |
||
600 |
|||
601 |
if (arg == ARG0) { |
||
602 |
PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH |
||
603 |
PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED |
||
604 |
✓✗ | 94 |
JacobianMatrix_t J1; |
605 |
✓✗ | 94 |
Jlog6 (M, J1); |
606 |
PINOCCHIO_COMPILER_DIAGNOSTIC_POP |
||
607 |
|||
608 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
94 |
const Vector3 p1_p0 = R1.transpose()*(q1.template head<3>() - q0.template head<3>()); |
609 |
|||
610 |
94 |
JacobianOut_t & J0 = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J); |
|
611 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
94 |
J0.template bottomRightCorner<3,3> ().noalias() = J0.template topLeftCorner <3,3> ().noalias() = - M.rotation().transpose(); |
612 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
94 |
J0.template topRightCorner<3,3> ().noalias() = skew(p1_p0) * M.rotation().transpose(); // = R1.T * skew(q1_t - q0_t) * R0; |
613 |
✓✗✓✗ |
94 |
J0.template bottomLeftCorner<3,3> ().setZero(); |
614 |
✓✗ | 94 |
J0.applyOnTheLeft(J1); |
615 |
} |
||
616 |
else if (arg == ARG1) { |
||
617 |
✓✗ | 134 |
Jlog6 (M, J); |
618 |
} |
||
619 |
228 |
} |
|
620 |
|||
621 |
template <class ConfigIn_t, class Velocity_t, class ConfigOut_t> |
||
622 |
14141 |
static void integrate_impl(const Eigen::MatrixBase<ConfigIn_t> & q, |
|
623 |
const Eigen::MatrixBase<Velocity_t> & v, |
||
624 |
const Eigen::MatrixBase<ConfigOut_t> & qout) |
||
625 |
{ |
||
626 |
14141 |
ConfigOut_t & out = PINOCCHIO_EIGEN_CONST_CAST(ConfigOut_t,qout); |
|
627 |
✓✗✓✗ |
14141 |
Quaternion_t const quat(q.derived().template tail<4>()); |
628 |
✓✗✗✓ |
14141 |
assert(quaternion::isNormalized(quat,RealScalar(PINOCCHIO_DEFAULT_QUATERNION_NORM_TOLERANCE_VALUE))); |
629 |
✓✗✓✗ |
14141 |
QuaternionMap_t res_quat (out.template tail<4>().data()); |
630 |
|||
631 |
using internal::if_then_else; |
||
632 |
|||
633 |
✓✗✓✗ ✓✗ |
14141 |
SE3 M0 (quat.matrix(), q.derived().template head<3>()); |
634 |
✓✓✗✓ ✗ |
14141 |
MotionRef<const Velocity_t> mref_v(v.derived()); |
635 |
✓✗✓✗ |
14141 |
SE3 M1 (M0 * exp6(mref_v)); |
636 |
|||
637 |
✓✗✓✗ ✓✗ |
14141 |
out.template head<3>() = M1.translation(); |
638 |
✓✗✓✗ |
14141 |
quaternion::assignQuaternion(res_quat,M1.rotation()); // required by CasADi |
639 |
✓✗ | 14141 |
const Scalar dot_product = res_quat.dot(quat); |
640 |
✓✓ | 70705 |
for(Eigen::DenseIndex k = 0; k < 4; ++k) |
641 |
{ |
||
642 |
✓✗✓✗ ✓✗ |
113128 |
res_quat.coeffs().coeffRef(k) = if_then_else(internal::LT, dot_product, Scalar(0), |
643 |
✓✗ | 56564 |
-res_quat.coeffs().coeff(k), |
644 |
56564 |
res_quat.coeffs().coeff(k)); |
|
645 |
} |
||
646 |
|||
647 |
// Norm of qs might be epsilon-different to 1, so M1.rotation might be epsilon-different to a rotation matrix. |
||
648 |
// It is then safer to re-normalized after converting M1.rotation to quaternion. |
||
649 |
✓✗ | 14141 |
quaternion::firstOrderNormalize(res_quat); |
650 |
✓✗✗✓ |
14141 |
assert(quaternion::isNormalized(res_quat,RealScalar(PINOCCHIO_DEFAULT_QUATERNION_NORM_TOLERANCE_VALUE))); |
651 |
14141 |
} |
|
652 |
|||
653 |
template <class Config_t, class Jacobian_t> |
||
654 |
22 |
static void integrateCoeffWiseJacobian_impl(const Eigen::MatrixBase<Config_t> & q, |
|
655 |
const Eigen::MatrixBase<Jacobian_t> & J) |
||
656 |
{ |
||
657 |
✓✗✓✗ ✓✗✓✗ |
22 |
assert(J.rows() == nq() && J.cols() == nv() && "J is not of the right dimension"); |
658 |
|||
659 |
typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(Config_t) ConfigPlainType; |
||
660 |
typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(Jacobian_t) JacobianPlainType; |
||
661 |
typedef typename ConfigPlainType::Scalar Scalar; |
||
662 |
|||
663 |
22 |
Jacobian_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t,J); |
|
664 |
✓✗ | 22 |
Jout.setZero(); |
665 |
|||
666 |
✓✗✓✗ |
22 |
ConstQuaternionMap_t quat_map(q.derived().template tail<4>().data()); |
667 |
✓✗✗✓ |
22 |
assert(quaternion::isNormalized(quat_map,RealScalar(PINOCCHIO_DEFAULT_QUATERNION_NORM_TOLERANCE_VALUE))); |
668 |
✓✗✓✗ ✓✗ |
22 |
Jout.template topLeftCorner<3,3>() = quat_map.toRotationMatrix(); |
669 |
// Jexp3(quat,Jout.template bottomRightCorner<4,3>()); |
||
670 |
|||
671 |
typedef Eigen::Matrix<Scalar,4,3,JacobianPlainType::Options|Eigen::RowMajor> Jacobian43; |
||
672 |
typedef SE3Tpl<Scalar,ConfigPlainType::Options> SE3; |
||
673 |
PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH |
||
674 |
PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED |
||
675 |
✓✗ | 22 |
Jacobian43 Jexp3QuatCoeffWise; |
676 |
|||
677 |
Scalar theta; |
||
678 |
✓✗ | 22 |
typename SE3::Vector3 v = quaternion::log3(quat_map,theta); |
679 |
✓✗ | 22 |
quaternion::Jexp3CoeffWise(v,Jexp3QuatCoeffWise); |
680 |
✓✗ | 22 |
typename SE3::Matrix3 Jlog; |
681 |
✓✗ | 22 |
Jlog3(theta,v,Jlog); |
682 |
PINOCCHIO_COMPILER_DIAGNOSTIC_POP |
||
683 |
|||
684 |
// std::cout << "Jexp3QuatCoeffWise\n" << Jexp3QuatCoeffWise << std::endl; |
||
685 |
// std::cout << "Jlog\n" << Jlog << std::endl; |
||
686 |
|||
687 |
// if(quat_map.w() >= 0.) // comes from the log3 for quaternions which may change the sign. |
||
688 |
✓✗✓✓ |
22 |
if(quat_map.coeffs()[3] >= Scalar(0)) // comes from the log3 for quaternions which may change the sign. |
689 |
✓✗✓✗ ✓✗✓✗ |
13 |
PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t,J).template bottomRightCorner<4,3>().noalias() = Jexp3QuatCoeffWise * Jlog; |
690 |
else |
||
691 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
9 |
PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t,J).template bottomRightCorner<4,3>().noalias() = -Jexp3QuatCoeffWise * Jlog; |
692 |
22 |
} |
|
693 |
|||
694 |
template <class Config_t, class Tangent_t, class JacobianOut_t> |
||
695 |
40 |
static void dIntegrate_dq_impl(const Eigen::MatrixBase<Config_t > & /*q*/, |
|
696 |
const Eigen::MatrixBase<Tangent_t> & v, |
||
697 |
const Eigen::MatrixBase<JacobianOut_t>& J, |
||
698 |
const AssignmentOperatorType op=SETTO) |
||
699 |
{ |
||
700 |
40 |
JacobianOut_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J); |
|
701 |
|||
702 |
✓✓✓✗ |
40 |
switch(op) |
703 |
{ |
||
704 |
38 |
case SETTO: |
|
705 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
38 |
Jout = exp6(MotionRef<const Tangent_t>(v.derived())).toDualActionMatrix().transpose(); |
706 |
38 |
break; |
|
707 |
1 |
case ADDTO: |
|
708 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
1 |
Jout += exp6(MotionRef<const Tangent_t>(v.derived())).toDualActionMatrix().transpose(); |
709 |
1 |
break; |
|
710 |
1 |
case RMTO: |
|
711 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
1 |
Jout -= exp6(MotionRef<const Tangent_t>(v.derived())).toDualActionMatrix().transpose(); |
712 |
1 |
break; |
|
713 |
default: |
||
714 |
assert(false && "Wrong Op requesed value"); |
||
715 |
break; |
||
716 |
} |
||
717 |
40 |
} |
|
718 |
|||
719 |
template <class Config_t, class Tangent_t, class JacobianOut_t> |
||
720 |
60 |
static void dIntegrate_dv_impl(const Eigen::MatrixBase<Config_t > & /*q*/, |
|
721 |
const Eigen::MatrixBase<Tangent_t> & v, |
||
722 |
const Eigen::MatrixBase<JacobianOut_t>& J, |
||
723 |
const AssignmentOperatorType op=SETTO) |
||
724 |
{ |
||
725 |
✓✓✓✗ |
60 |
switch(op) |
726 |
{ |
||
727 |
58 |
case SETTO: |
|
728 |
✓✗✓✗ |
58 |
Jexp6<SETTO>(MotionRef<const Tangent_t>(v.derived()), J.derived()); |
729 |
58 |
break; |
|
730 |
1 |
case ADDTO: |
|
731 |
✓✗✓✗ |
1 |
Jexp6<ADDTO>(MotionRef<const Tangent_t>(v.derived()), J.derived()); |
732 |
1 |
break; |
|
733 |
1 |
case RMTO: |
|
734 |
✓✗✓✗ |
1 |
Jexp6<RMTO>(MotionRef<const Tangent_t>(v.derived()), J.derived()); |
735 |
1 |
break; |
|
736 |
default: |
||
737 |
assert(false && "Wrong Op requesed value"); |
||
738 |
break; |
||
739 |
} |
||
740 |
60 |
} |
|
741 |
|||
742 |
template <class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t> |
||
743 |
1 |
static void dIntegrateTransport_dq_impl(const Eigen::MatrixBase<Config_t > & /*q*/, |
|
744 |
const Eigen::MatrixBase<Tangent_t> & v, |
||
745 |
const Eigen::MatrixBase<JacobianIn_t> & Jin, |
||
746 |
const Eigen::MatrixBase<JacobianOut_t> & J_out) |
||
747 |
{ |
||
748 |
1 |
JacobianOut_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J_out); |
|
749 |
✓✗ | 1 |
Eigen::Matrix<Scalar,6,6> Jtmp6; |
750 |
✗✓✗✗ ✓✗✗✓ ✗✗✓✗ ✗✓✗✗ ✗ |
1 |
Jtmp6 = exp6(MotionRef<const Tangent_t>(v.derived())).toDualActionMatrix().transpose(); |
751 |
|||
752 |
Jout.template topRows<3>().noalias() |
||
753 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
1 |
= Jtmp6.template topLeftCorner<3,3>() * Jin.template topRows<3>(); |
754 |
Jout.template topRows<3>().noalias() |
||
755 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
1 |
+= Jtmp6.template topRightCorner<3,3>() * Jin.template bottomRows<3>(); |
756 |
Jout.template bottomRows<3>().noalias() |
||
757 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
1 |
= Jtmp6.template bottomRightCorner<3,3>() * Jin.template bottomRows<3>(); |
758 |
1 |
} |
|
759 |
|||
760 |
template <class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t> |
||
761 |
1 |
static void dIntegrateTransport_dv_impl(const Eigen::MatrixBase<Config_t > & /*q*/, |
|
762 |
const Eigen::MatrixBase<Tangent_t> & v, |
||
763 |
const Eigen::MatrixBase<JacobianIn_t> & Jin, |
||
764 |
const Eigen::MatrixBase<JacobianOut_t> & J_out) |
||
765 |
{ |
||
766 |
1 |
JacobianOut_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J_out); |
|
767 |
PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH |
||
768 |
PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED |
||
769 |
✓✗ | 1 |
Eigen::Matrix<Scalar,6,6> Jtmp6; |
770 |
✗✓✗✗ ✓✗✗✗ |
1 |
Jexp6<SETTO>(MotionRef<const Tangent_t>(v.derived()), Jtmp6); |
771 |
PINOCCHIO_COMPILER_DIAGNOSTIC_POP |
||
772 |
|||
773 |
Jout.template topRows<3>().noalias() |
||
774 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
1 |
= Jtmp6.template topLeftCorner<3,3>() * Jin.template topRows<3>(); |
775 |
Jout.template topRows<3>().noalias() |
||
776 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
1 |
+= Jtmp6.template topRightCorner<3,3>() * Jin.template bottomRows<3>(); |
777 |
Jout.template bottomRows<3>().noalias() |
||
778 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
1 |
= Jtmp6.template bottomRightCorner<3,3>() * Jin.template bottomRows<3>(); |
779 |
1 |
} |
|
780 |
|||
781 |
|||
782 |
template <class Config_t, class Tangent_t, class Jacobian_t> |
||
783 |
1 |
static void dIntegrateTransport_dq_impl(const Eigen::MatrixBase<Config_t > & /*q*/, |
|
784 |
const Eigen::MatrixBase<Tangent_t> & v, |
||
785 |
const Eigen::MatrixBase<Jacobian_t> & J_out) |
||
786 |
{ |
||
787 |
1 |
Jacobian_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t,J_out); |
|
788 |
✓✗ | 1 |
Eigen::Matrix<Scalar,6,6> Jtmp6; |
789 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
1 |
Jtmp6 = exp6(MotionRef<const Tangent_t>(v.derived())).toDualActionMatrix().transpose(); |
790 |
|||
791 |
//Aliasing |
||
792 |
Jout.template topRows<3>() |
||
793 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
1 |
= Jtmp6.template topLeftCorner<3,3>() * Jout.template topRows<3>(); |
794 |
Jout.template topRows<3>().noalias() |
||
795 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
1 |
+= Jtmp6.template topRightCorner<3,3>() * Jout.template bottomRows<3>(); |
796 |
Jout.template bottomRows<3>() |
||
797 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
1 |
= Jtmp6.template bottomRightCorner<3,3>() * Jout.template bottomRows<3>(); |
798 |
1 |
} |
|
799 |
|||
800 |
template <class Config_t, class Tangent_t, class Jacobian_t> |
||
801 |
1 |
static void dIntegrateTransport_dv_impl(const Eigen::MatrixBase<Config_t > & /*q*/, |
|
802 |
const Eigen::MatrixBase<Tangent_t> & v, |
||
803 |
const Eigen::MatrixBase<Jacobian_t> & J_out) |
||
804 |
{ |
||
805 |
1 |
Jacobian_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t,J_out); |
|
806 |
PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH |
||
807 |
PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED |
||
808 |
✓✗ | 1 |
Eigen::Matrix<Scalar,6,6> Jtmp6; |
809 |
✓✗✓✗ |
1 |
Jexp6<SETTO>(MotionRef<const Tangent_t>(v.derived()), Jtmp6); |
810 |
PINOCCHIO_COMPILER_DIAGNOSTIC_POP |
||
811 |
|||
812 |
Jout.template topRows<3>() |
||
813 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
1 |
= Jtmp6.template topLeftCorner<3,3>() * Jout.template topRows<3>(); |
814 |
Jout.template topRows<3>().noalias() |
||
815 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
1 |
+= Jtmp6.template topRightCorner<3,3>() * Jout.template bottomRows<3>(); |
816 |
Jout.template bottomRows<3>() |
||
817 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
1 |
= Jtmp6.template bottomRightCorner<3,3>() * Jout.template bottomRows<3>(); |
818 |
1 |
} |
|
819 |
|||
820 |
template <class ConfigL_t, class ConfigR_t> |
||
821 |
2063 |
static Scalar squaredDistance_impl(const Eigen::MatrixBase<ConfigL_t> & q0, |
|
822 |
const Eigen::MatrixBase<ConfigR_t> & q1) |
||
823 |
{ |
||
824 |
PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH |
||
825 |
PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED |
||
826 |
✓✗ | 2063 |
TangentVector_t t; |
827 |
✓✗ | 2063 |
difference_impl(q0, q1, t); |
828 |
PINOCCHIO_COMPILER_DIAGNOSTIC_POP |
||
829 |
✓✗ | 2063 |
return t.squaredNorm(); |
830 |
} |
||
831 |
|||
832 |
template <class Config_t> |
||
833 |
2065 |
static void normalize_impl (const Eigen::MatrixBase<Config_t>& qout) |
|
834 |
{ |
||
835 |
2065 |
Config_t & qout_ = PINOCCHIO_EIGEN_CONST_CAST(Config_t,qout); |
|
836 |
✓✗ | 2065 |
qout_.template tail<4>().normalize(); |
837 |
2065 |
} |
|
838 |
|||
839 |
template <class Config_t> |
||
840 |
10211 |
static bool isNormalized_impl(const Eigen::MatrixBase<Config_t>& qin, |
|
841 |
const Scalar& prec) |
||
842 |
{ |
||
843 |
✓✗ | 10211 |
Scalar norm = Scalar(qin.template tail<4>().norm()); |
844 |
using std::abs; |
||
845 |
10211 |
return abs(norm - Scalar(1.0)) < prec; |
|
846 |
} |
||
847 |
|||
848 |
template <class Config_t> |
||
849 |
305 |
static void random_impl (const Eigen::MatrixBase<Config_t>& qout) |
|
850 |
{ |
||
851 |
✓✗ | 305 |
R3crossSO3_t().random(qout); |
852 |
305 |
} |
|
853 |
|||
854 |
template <class ConfigL_t, class ConfigR_t, class ConfigOut_t> |
||
855 |
6230 |
static void randomConfiguration_impl(const Eigen::MatrixBase<ConfigL_t> & lower, |
|
856 |
const Eigen::MatrixBase<ConfigR_t> & upper, |
||
857 |
const Eigen::MatrixBase<ConfigOut_t> & qout) |
||
858 |
{ |
||
859 |
✓✗ | 6230 |
R3crossSO3_t ().randomConfiguration(lower, upper, qout); |
860 |
6230 |
} |
|
861 |
|||
862 |
template <class ConfigL_t, class ConfigR_t> |
||
863 |
5 |
static bool isSameConfiguration_impl(const Eigen::MatrixBase<ConfigL_t> & q0, |
|
864 |
const Eigen::MatrixBase<ConfigR_t> & q1, |
||
865 |
const Scalar & prec) |
||
866 |
{ |
||
867 |
✓✗ | 5 |
return R3crossSO3_t().isSameConfiguration(q0, q1, prec); |
868 |
} |
||
869 |
}; // struct SpecialEuclideanOperationTpl<3> |
||
870 |
|||
871 |
} // namespace pinocchio |
||
872 |
|||
873 |
#endif // ifndef __pinocchio_multibody_liegroup_special_euclidean_operation_hpp__ |
| Generated by: GCOVR (Version 4.2) |