| 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 |
✓✗✓✗ ✓✗✓✗ |
10987 |
Matrix2 R0, R1; Vector2 t0, t1; |
226 |
✓✗ | 10987 |
forwardKinematics(R0, t0, q0); |
227 |
✓✗ | 10987 |
forwardKinematics(R1, t1, q1); |
228 |
✓✗✓✗ ✓✗ |
10987 |
Matrix2 R (R0.transpose() * R1); |
229 |
✓✗✓✗ ✓✗✓✗ |
10987 |
Vector2 t (R0.transpose() * (t1 - t0)); |
230 |
|||
231 |
✓✗ | 10987 |
log(R, t, d); |
232 |
10987 |
} |
|
233 |
|||
234 |
template <ArgumentPosition arg, class ConfigL_t, class ConfigR_t, class JacobianOut_t> |
||
235 |
140 |
void dDifference_impl(const Eigen::MatrixBase<ConfigL_t> & q0, |
|
236 |
const Eigen::MatrixBase<ConfigR_t> & q1, |
||
237 |
const Eigen::MatrixBase<JacobianOut_t>& J) const |
||
238 |
{ |
||
239 |
✓✗✓✗ ✓✗✓✗ |
140 |
Matrix2 R0, R1; Vector2 t0, t1; |
240 |
✓✗ | 140 |
forwardKinematics(R0, t0, q0); |
241 |
✓✗ | 140 |
forwardKinematics(R1, t1, q1); |
242 |
✓✗✓✗ ✓✗ |
140 |
Matrix2 R (R0.transpose() * R1); |
243 |
✓✗✓✗ ✓✗✓✗ |
140 |
Vector2 t (R0.transpose() * (t1 - t0)); |
244 |
|||
245 |
if (arg == ARG0) { |
||
246 |
✓✗ | 50 |
JacobianMatrix_t J1; |
247 |
✓✗ | 50 |
Jlog (R, t, J1); |
248 |
|||
249 |
// pcross = [ y1-y0, - (x1 - x0) ] |
||
250 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
50 |
Vector2 pcross (q1(1) - q0(1), q0(0) - q1(0)); |
251 |
|||
252 |
50 |
JacobianOut_t& J0 = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, J); |
|
253 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
50 |
J0.template topLeftCorner <2,2> ().noalias() = - R.transpose(); |
254 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
50 |
J0.template topRightCorner<2,1> ().noalias() = R1.transpose() * pcross; |
255 |
✓✗✓✗ |
50 |
J0.template bottomLeftCorner <1,2> ().setZero(); |
256 |
✓✗ | 50 |
J0 (2,2) = -1; |
257 |
✓✗ | 50 |
J0.applyOnTheLeft(J1); |
258 |
} else if (arg == ARG1) { |
||
259 |
✓✗ | 90 |
Jlog (R, t, J); |
260 |
} |
||
261 |
140 |
} |
|
262 |
|||
263 |
template <class ConfigIn_t, class Velocity_t, class ConfigOut_t> |
||
264 |
11311 |
static void integrate_impl(const Eigen::MatrixBase<ConfigIn_t> & q, |
|
265 |
const Eigen::MatrixBase<Velocity_t> & v, |
||
266 |
const Eigen::MatrixBase<ConfigOut_t> & qout) |
||
267 |
{ |
||
268 |
11311 |
ConfigOut_t & out = PINOCCHIO_EIGEN_CONST_CAST(ConfigOut_t,qout); |
|
269 |
|||
270 |
✓✗✓✗ |
11311 |
Matrix2 R0, R; |
271 |
✓✗✓✗ |
11311 |
Vector2 t0, t; |
272 |
✓✗ | 11311 |
forwardKinematics(R0, t0, q); |
273 |
✓✗ | 11311 |
exp(v, R, t); |
274 |
|||
275 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
11311 |
out.template head<2>().noalias() = R0 * t + t0; |
276 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
11311 |
out.template tail<2>().noalias() = R0 * R.col(0); |
277 |
11311 |
} |
|
278 |
|||
279 |
template <class Config_t, class Jacobian_t> |
||
280 |
20 |
static void integrateCoeffWiseJacobian_impl(const Eigen::MatrixBase<Config_t> & q, |
|
281 |
const Eigen::MatrixBase<Jacobian_t> & J) |
||
282 |
{ |
||
283 |
✓✗✓✗ |
20 |
assert(J.rows() == nq() && J.cols() == nv() && "J is not of the right dimension"); |
284 |
|||
285 |
20 |
Jacobian_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t,J); |
|
286 |
20 |
Jout.setZero(); |
|
287 |
|||
288 |
20 |
const typename Config_t::Scalar & c_theta = q(2), |
|
289 |
20 |
& s_theta = q(3); |
|
290 |
|||
291 |
✓✗✓✗ ✓✗✓✗ |
20 |
Jout.template topLeftCorner<2,2>() << c_theta, -s_theta, s_theta, c_theta; |
292 |
✓✗✓✗ |
20 |
Jout.template bottomRightCorner<2,1>() << -s_theta, c_theta; |
293 |
20 |
} |
|
294 |
|||
295 |
template <class Config_t, class Tangent_t, class JacobianOut_t> |
||
296 |
36 |
static void dIntegrate_dq_impl(const Eigen::MatrixBase<Config_t > & /*q*/, |
|
297 |
const Eigen::MatrixBase<Tangent_t> & v, |
||
298 |
const Eigen::MatrixBase<JacobianOut_t>& J, |
||
299 |
const AssignmentOperatorType op=SETTO) |
||
300 |
{ |
||
301 |
36 |
JacobianOut_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J); |
|
302 |
|||
303 |
✓✗ | 36 |
Matrix2 R; |
304 |
✓✗ | 36 |
Vector2 t; |
305 |
✓✗ | 36 |
exp(v, R, t); |
306 |
|||
307 |
✓✗ | 36 |
toInverseActionMatrix(R, t, Jout, op); |
308 |
36 |
} |
|
309 |
|||
310 |
template <class Config_t, class Tangent_t, class JacobianOut_t> |
||
311 |
56 |
static void dIntegrate_dv_impl(const Eigen::MatrixBase<Config_t > & /*q*/, |
|
312 |
const Eigen::MatrixBase<Tangent_t> & v, |
||
313 |
const Eigen::MatrixBase<JacobianOut_t> & J, |
||
314 |
const AssignmentOperatorType op=SETTO) |
||
315 |
{ |
||
316 |
56 |
JacobianOut_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J); |
|
317 |
// TODO sparse version |
||
318 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
56 |
MotionTpl<Scalar,0> nu; nu.toVector() << v.template head<2>(), 0, 0, 0, v[2]; |
319 |
✓✗ | 56 |
Eigen::Matrix<Scalar,6,6> Jtmp6; |
320 |
✓✗ | 56 |
Jexp6(nu, Jtmp6); |
321 |
|||
322 |
✓✓✓✗ |
56 |
switch(op) |
323 |
{ |
||
324 |
54 |
case SETTO: |
|
325 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
54 |
Jout << Jtmp6.template topLeftCorner<2,2>(), Jtmp6.template topRightCorner<2,1>(), |
326 |
✓✗✓✗ |
54 |
Jtmp6.template bottomLeftCorner<1,2>(), Jtmp6.template bottomRightCorner<1,1>(); |
327 |
54 |
break; |
|
328 |
1 |
case ADDTO: |
|
329 |
✓✗✓✗ ✓✗ |
1 |
Jout.template topLeftCorner<2,2>() += Jtmp6.template topLeftCorner<2,2>(); |
330 |
✓✗✓✗ ✓✗ |
1 |
Jout.template topRightCorner<2,1>() += Jtmp6.template topRightCorner<2,1>(); |
331 |
✓✗✓✗ ✓✗ |
1 |
Jout.template bottomLeftCorner<1,2>() += Jtmp6.template bottomLeftCorner<1,2>(); |
332 |
✓✗✓✗ ✓✗ |
1 |
Jout.template bottomRightCorner<1,1>() += Jtmp6.template bottomRightCorner<1,1>(); |
333 |
1 |
break; |
|
334 |
1 |
case RMTO: |
|
335 |
✓✗✓✗ ✓✗ |
1 |
Jout.template topLeftCorner<2,2>() -= Jtmp6.template topLeftCorner<2,2>(); |
336 |
✓✗✓✗ ✓✗ |
1 |
Jout.template topRightCorner<2,1>() -= Jtmp6.template topRightCorner<2,1>(); |
337 |
✓✗✓✗ ✓✗ |
1 |
Jout.template bottomLeftCorner<1,2>() -= Jtmp6.template bottomLeftCorner<1,2>(); |
338 |
✓✗✓✗ ✓✗ |
1 |
Jout.template bottomRightCorner<1,1>() -= Jtmp6.template bottomRightCorner<1,1>(); |
339 |
1 |
break; |
|
340 |
default: |
||
341 |
assert(false && "Wrong Op requesed value"); |
||
342 |
break; |
||
343 |
} |
||
344 |
56 |
} |
|
345 |
|||
346 |
template <class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t> |
||
347 |
1 |
void dIntegrateTransport_dq_impl(const Eigen::MatrixBase<Config_t > & /*q*/, |
|
348 |
const Eigen::MatrixBase<Tangent_t> & v, |
||
349 |
const Eigen::MatrixBase<JacobianIn_t> & Jin, |
||
350 |
const Eigen::MatrixBase<JacobianOut_t> & J_out) const |
||
351 |
{ |
||
352 |
1 |
JacobianOut_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J_out); |
|
353 |
✓✗ | 1 |
Matrix2 R; |
354 |
✓✗ | 1 |
Vector2 t; |
355 |
✓✗ | 1 |
exp(v, R, t); |
356 |
|||
357 |
✓✗✓✗ ✓✗✓✗ |
1 |
Vector2 tinv = (R.transpose() * t).reverse(); |
358 |
✓✗ | 1 |
tinv[0] *= Scalar(-1.); |
359 |
|||
360 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
1 |
Jout.template topRows<2>().noalias() = R.transpose() * Jin.template topRows<2>(); |
361 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
1 |
Jout.template topRows<2>().noalias() += tinv * Jin.template bottomRows<1>(); |
362 |
✓✗✓✗ ✓✗ |
1 |
Jout.template bottomRows<1>() = Jin.template bottomRows<1>(); |
363 |
1 |
} |
|
364 |
|||
365 |
template <class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t> |
||
366 |
1 |
void dIntegrateTransport_dv_impl(const Eigen::MatrixBase<Config_t > & /*q*/, |
|
367 |
const Eigen::MatrixBase<Tangent_t> & v, |
||
368 |
const Eigen::MatrixBase<JacobianIn_t> & Jin, |
||
369 |
const Eigen::MatrixBase<JacobianOut_t> & J_out) const |
||
370 |
{ |
||
371 |
1 |
JacobianOut_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J_out); |
|
372 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
1 |
MotionTpl<Scalar,0> nu; nu.toVector() << v.template head<2>(), 0, 0, 0, v[2]; |
373 |
|||
374 |
✓✗ | 1 |
Eigen::Matrix<Scalar,6,6> Jtmp6; |
375 |
✓✗ | 1 |
Jexp6(nu, Jtmp6); |
376 |
|||
377 |
Jout.template topRows<2>().noalias() |
||
378 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
1 |
= Jtmp6.template topLeftCorner<2,2>() * Jin.template topRows<2>(); |
379 |
Jout.template topRows<2>().noalias() |
||
380 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
1 |
+= Jtmp6.template topRightCorner<2,1>() * Jin.template bottomRows<1>(); |
381 |
Jout.template bottomRows<1>().noalias() |
||
382 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
1 |
= Jtmp6.template bottomLeftCorner<1,2>()* Jin.template topRows<2>(); |
383 |
Jout.template bottomRows<1>().noalias() |
||
384 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
1 |
+= Jtmp6.template bottomRightCorner<1,1>() * Jin.template bottomRows<1>(); |
385 |
|||
386 |
1 |
} |
|
387 |
|||
388 |
template <class Config_t, class Tangent_t, class Jacobian_t> |
||
389 |
1 |
void dIntegrateTransport_dq_impl(const Eigen::MatrixBase<Config_t > & /*q*/, |
|
390 |
const Eigen::MatrixBase<Tangent_t> & v, |
||
391 |
const Eigen::MatrixBase<Jacobian_t> & J) const |
||
392 |
{ |
||
393 |
1 |
Jacobian_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t,J); |
|
394 |
✓✗ | 1 |
Matrix2 R; |
395 |
✓✗ | 1 |
Vector2 t; |
396 |
✓✗ | 1 |
exp(v, R, t); |
397 |
|||
398 |
✓✗✓✗ ✓✗✓✗ |
1 |
Vector2 tinv = (R.transpose() * t).reverse(); |
399 |
✓✗ | 1 |
tinv[0] *= Scalar(-1); |
400 |
//TODO: Aliasing |
||
401 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
1 |
Jout.template topRows<2>() = R.transpose() * Jout.template topRows<2>(); |
402 |
//No Aliasing |
||
403 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
1 |
Jout.template topRows<2>().noalias() += tinv * Jout.template bottomRows<1>(); |
404 |
1 |
} |
|
405 |
|||
406 |
template <class Config_t, class Tangent_t, class Jacobian_t> |
||
407 |
1 |
void dIntegrateTransport_dv_impl(const Eigen::MatrixBase<Config_t > & /*q*/, |
|
408 |
const Eigen::MatrixBase<Tangent_t> & v, |
||
409 |
const Eigen::MatrixBase<Jacobian_t> & J) const |
||
410 |
{ |
||
411 |
1 |
Jacobian_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t,J); |
|
412 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
1 |
MotionTpl<Scalar,0> nu; nu.toVector() << v.template head<2>(), 0, 0, 0, v[2]; |
413 |
|||
414 |
✓✗ | 1 |
Eigen::Matrix<Scalar,6,6> Jtmp6; |
415 |
✓✗ | 1 |
Jexp6(nu, Jtmp6); |
416 |
//TODO: Remove aliasing |
||
417 |
Jout.template topRows<2>() |
||
418 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
1 |
= Jtmp6.template topLeftCorner<2,2>() * Jout.template topRows<2>(); |
419 |
Jout.template topRows<2>().noalias() |
||
420 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
1 |
+= Jtmp6.template topRightCorner<2,1>() * Jout.template bottomRows<1>(); |
421 |
Jout.template bottomRows<1>() |
||
422 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
1 |
= Jtmp6.template bottomRightCorner<1,1>() * Jout.template bottomRows<1>(); |
423 |
Jout.template bottomRows<1>().noalias() |
||
424 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
1 |
+= Jtmp6.template bottomLeftCorner<1,2>() * Jout.template topRows<2>(); |
425 |
1 |
} |
|
426 |
|||
427 |
template <class Config_t> |
||
428 |
2054 |
static void normalize_impl(const Eigen::MatrixBase<Config_t> & qout) |
|
429 |
{ |
||
430 |
✓✗ | 2054 |
PINOCCHIO_EIGEN_CONST_CAST(Config_t,qout).template tail<2>().normalize(); |
431 |
2054 |
} |
|
432 |
|||
433 |
template <class Config_t> |
||
434 |
10209 |
static bool isNormalized_impl(const Eigen::MatrixBase<Config_t> & qin, |
|
435 |
const Scalar& prec) |
||
436 |
{ |
||
437 |
✓✗ | 10209 |
const Scalar norm = Scalar(qin.template tail<2>().norm()); |
438 |
using std::abs; |
||
439 |
10209 |
return abs(norm - Scalar(1.0)) < prec; |
|
440 |
} |
||
441 |
|||
442 |
template <class Config_t> |
||
443 |
217 |
void random_impl(const Eigen::MatrixBase<Config_t> & qout) const |
|
444 |
{ |
||
445 |
✓✗ | 217 |
R2crossSO2_t().random(qout); |
446 |
217 |
} |
|
447 |
|||
448 |
template <class ConfigL_t, class ConfigR_t, class ConfigOut_t> |
||
449 |
6151 |
void randomConfiguration_impl(const Eigen::MatrixBase<ConfigL_t> & lower, |
|
450 |
const Eigen::MatrixBase<ConfigR_t> & upper, |
||
451 |
const Eigen::MatrixBase<ConfigOut_t> & qout) |
||
452 |
const |
||
453 |
{ |
||
454 |
✓✗ | 6151 |
R2crossSO2_t ().randomConfiguration(lower, upper, qout); |
455 |
6151 |
} |
|
456 |
|||
457 |
template <class ConfigL_t, class ConfigR_t> |
||
458 |
5 |
static bool isSameConfiguration_impl(const Eigen::MatrixBase<ConfigL_t> & q0, |
|
459 |
const Eigen::MatrixBase<ConfigR_t> & q1, |
||
460 |
const Scalar & prec) |
||
461 |
{ |
||
462 |
✓✗ | 5 |
return R2crossSO2_t().isSameConfiguration(q0, q1, prec); |
463 |
} |
||
464 |
|||
465 |
protected: |
||
466 |
|||
467 |
template<typename Matrix2Like, typename Vector2Like, typename Vector4Like> |
||
468 |
33179 |
static void forwardKinematics(const Eigen::MatrixBase<Matrix2Like> & R, |
|
469 |
const Eigen::MatrixBase<Vector2Like> & t, |
||
470 |
const Eigen::MatrixBase<Vector4Like> & q) |
||
471 |
{ |
||
472 |
EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(Matrix2Like, Matrix2); |
||
473 |
EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(Vector2Like, Vector2); |
||
474 |
EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigVector_t,Vector4Like); |
||
475 |
|||
476 |
✓✗ | 33179 |
PINOCCHIO_EIGEN_CONST_CAST(Vector2Like,t) = q.template head<2>(); |
477 |
33179 |
const typename Vector4Like::Scalar & c_theta = q(2), |
|
478 |
33179 |
& s_theta = q(3); |
|
479 |
✓✗✓✗ ✓✗ |
33179 |
PINOCCHIO_EIGEN_CONST_CAST(Matrix2Like,R) << c_theta, -s_theta, s_theta, c_theta; |
480 |
|||
481 |
33179 |
} |
|
482 |
}; // struct SpecialEuclideanOperationTpl<2> |
||
483 |
|||
484 |
template<typename _Scalar, int _Options> |
||
485 |
struct traits< SpecialEuclideanOperationTpl<3,_Scalar,_Options> > |
||
486 |
{ |
||
487 |
typedef _Scalar Scalar; |
||
488 |
enum |
||
489 |
{ |
||
490 |
Options = _Options, |
||
491 |
NQ = 7, |
||
492 |
NV = 6 |
||
493 |
}; |
||
494 |
}; |
||
495 |
|||
496 |
/// SE(3) |
||
497 |
template<typename _Scalar, int _Options> |
||
498 |
struct SpecialEuclideanOperationTpl<3,_Scalar,_Options> |
||
499 |
: public LieGroupBase <SpecialEuclideanOperationTpl<3,_Scalar,_Options> > |
||
500 |
{ |
||
501 |
PINOCCHIO_LIE_GROUP_TPL_PUBLIC_INTERFACE(SpecialEuclideanOperationTpl); |
||
502 |
|||
503 |
typedef CartesianProductOperation <VectorSpaceOperationTpl<3,Scalar,Options>, SpecialOrthogonalOperationTpl<3,Scalar,Options> > R3crossSO3_t; |
||
504 |
|||
505 |
typedef Eigen::Quaternion<Scalar,Options> Quaternion_t; |
||
506 |
typedef Eigen::Map< Quaternion_t> QuaternionMap_t; |
||
507 |
typedef Eigen::Map<const Quaternion_t> ConstQuaternionMap_t; |
||
508 |
typedef SE3Tpl<Scalar,Options> Transformation_t; |
||
509 |
typedef SE3Tpl<Scalar,Options> SE3; |
||
510 |
typedef typename Eigen::NumTraits<Scalar>::Real RealScalar; |
||
511 |
|||
512 |
/// Get dimension of Lie Group vector representation |
||
513 |
/// |
||
514 |
/// For instance, for SO(3), the dimension of the vector representation is |
||
515 |
/// 4 (quaternion) while the dimension of the tangent space is 3. |
||
516 |
10056 |
static Index nq() |
|
517 |
{ |
||
518 |
10056 |
return NQ; |
|
519 |
} |
||
520 |
/// Get dimension of Lie Group tangent space |
||
521 |
4666 |
static Index nv() |
|
522 |
{ |
||
523 |
4666 |
return NV; |
|
524 |
} |
||
525 |
|||
526 |
25 |
static ConfigVector_t neutral() |
|
527 |
{ |
||
528 |
✓✗ | 25 |
ConfigVector_t n; n.template head<6>().setZero(); n[6] = 1; |
529 |
25 |
return n; |
|
530 |
} |
||
531 |
|||
532 |
67 |
static std::string name() |
|
533 |
{ |
||
534 |
✓✗ | 67 |
return std::string("SE(3)"); |
535 |
} |
||
536 |
|||
537 |
template <class ConfigL_t, class ConfigR_t, class Tangent_t> |
||
538 |
11532 |
static void difference_impl(const Eigen::MatrixBase<ConfigL_t> & q0, |
|
539 |
const Eigen::MatrixBase<ConfigR_t> & q1, |
||
540 |
const Eigen::MatrixBase<Tangent_t> & d) |
||
541 |
{ |
||
542 |
✓✗✓✗ |
11532 |
ConstQuaternionMap_t quat0 (q0.derived().template tail<4>().data()); |
543 |
✓✗✗✓ |
11532 |
assert(quaternion::isNormalized(quat0,RealScalar(PINOCCHIO_DEFAULT_QUATERNION_NORM_TOLERANCE_VALUE))); |
544 |
✓✗✓✗ |
11532 |
ConstQuaternionMap_t quat1 (q1.derived().template tail<4>().data()); |
545 |
✓✗✗✓ |
11532 |
assert(quaternion::isNormalized(quat1,RealScalar(PINOCCHIO_DEFAULT_QUATERNION_NORM_TOLERANCE_VALUE))); |
546 |
|||
547 |
11532 |
PINOCCHIO_EIGEN_CONST_CAST(Tangent_t,d) |
|
548 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
11532 |
= log6( SE3(quat0.matrix(), q0.derived().template head<3>()).inverse() |
549 |
11532 |
* SE3(quat1.matrix(), q1.derived().template head<3>())).toVector(); |
|
550 |
11532 |
} |
|
551 |
|||
552 |
/// \cheatsheet \f$ \frac{\partial\ominus}{\partial q_1} {}^1X_0 = - \frac{\partial\ominus}{\partial q_0} \f$ |
||
553 |
template <ArgumentPosition arg, class ConfigL_t, class ConfigR_t, class JacobianOut_t> |
||
554 |
228 |
void dDifference_impl (const Eigen::MatrixBase<ConfigL_t> & q0, |
|
555 |
const Eigen::MatrixBase<ConfigR_t> & q1, |
||
556 |
const Eigen::MatrixBase<JacobianOut_t> & J) const |
||
557 |
{ |
||
558 |
typedef typename SE3::Vector3 Vector3; |
||
559 |
typedef typename SE3::Matrix3 Matrix3; |
||
560 |
|||
561 |
✓✗✓✗ |
228 |
ConstQuaternionMap_t quat0 (q0.derived().template tail<4>().data()); |
562 |
✓✗✗✓ |
228 |
assert(quaternion::isNormalized(quat0,RealScalar(PINOCCHIO_DEFAULT_QUATERNION_NORM_TOLERANCE_VALUE))); |
563 |
✓✗✓✗ |
228 |
ConstQuaternionMap_t quat1 (q1.derived().template tail<4>().data()); |
564 |
✓✗✗✓ |
228 |
assert(quaternion::isNormalized(quat1,RealScalar(PINOCCHIO_DEFAULT_QUATERNION_NORM_TOLERANCE_VALUE))); |
565 |
|||
566 |
✓✗✓✗ |
228 |
Matrix3 R0(quat0.matrix()), R1 (quat1.matrix()); |
567 |
✓✗✗✓ ✓✗✗✓ |
228 |
assert(isUnitary(R0)); assert(isUnitary(R1)); |
568 |
|||
569 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
228 |
const SE3 M ( SE3(R0, q0.template head<3>()).inverse() |
570 |
* SE3(R1, q1.template head<3>())); |
||
571 |
|||
572 |
if (arg == ARG0) { |
||
573 |
✓✗ | 94 |
JacobianMatrix_t J1; |
574 |
✓✗ | 94 |
Jlog6 (M, J1); |
575 |
|||
576 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
94 |
const Vector3 p1_p0 = R1.transpose()*(q1.template head<3>() - q0.template head<3>()); |
577 |
|||
578 |
94 |
JacobianOut_t & J0 = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J); |
|
579 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
94 |
J0.template bottomRightCorner<3,3> ().noalias() = J0.template topLeftCorner <3,3> ().noalias() = - M.rotation().transpose(); |
580 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
94 |
J0.template topRightCorner<3,3> ().noalias() = skew(p1_p0) * M.rotation().transpose(); // = R1.T * skew(q1_t - q0_t) * R0; |
581 |
✓✗✓✗ |
94 |
J0.template bottomLeftCorner<3,3> ().setZero(); |
582 |
✓✗ | 94 |
J0.applyOnTheLeft(J1); |
583 |
} |
||
584 |
else if (arg == ARG1) { |
||
585 |
✓✗ | 134 |
Jlog6 (M, J); |
586 |
} |
||
587 |
228 |
} |
|
588 |
|||
589 |
template <class ConfigIn_t, class Velocity_t, class ConfigOut_t> |
||
590 |
14139 |
static void integrate_impl(const Eigen::MatrixBase<ConfigIn_t> & q, |
|
591 |
const Eigen::MatrixBase<Velocity_t> & v, |
||
592 |
const Eigen::MatrixBase<ConfigOut_t> & qout) |
||
593 |
{ |
||
594 |
14139 |
ConfigOut_t & out = PINOCCHIO_EIGEN_CONST_CAST(ConfigOut_t,qout); |
|
595 |
✓✗✓✗ |
14139 |
Quaternion_t const quat(q.derived().template tail<4>()); |
596 |
✓✗✗✓ |
14139 |
assert(quaternion::isNormalized(quat,RealScalar(PINOCCHIO_DEFAULT_QUATERNION_NORM_TOLERANCE_VALUE))); |
597 |
✓✗✓✗ |
14139 |
QuaternionMap_t res_quat (out.template tail<4>().data()); |
598 |
|||
599 |
using internal::if_then_else; |
||
600 |
|||
601 |
✓✗✓✗ ✓✗ |
14139 |
SE3 M0 (quat.matrix(), q.derived().template head<3>()); |
602 |
✓✓✗✓ ✗ |
14139 |
MotionRef<const Velocity_t> mref_v(v.derived()); |
603 |
✓✗✓✗ |
14139 |
SE3 M1 (M0 * exp6(mref_v)); |
604 |
|||
605 |
✓✗✓✗ ✓✗ |
14139 |
out.template head<3>() = M1.translation(); |
606 |
✓✗✓✗ |
14139 |
quaternion::assignQuaternion(res_quat,M1.rotation()); // required by CasADi |
607 |
✓✗ | 14139 |
const Scalar dot_product = res_quat.dot(quat); |
608 |
✓✓ | 70695 |
for(Eigen::DenseIndex k = 0; k < 4; ++k) |
609 |
{ |
||
610 |
✓✗✓✗ ✓✗ |
113112 |
res_quat.coeffs().coeffRef(k) = if_then_else(internal::LT, dot_product, Scalar(0), |
611 |
✓✗ | 56556 |
-res_quat.coeffs().coeff(k), |
612 |
56556 |
res_quat.coeffs().coeff(k)); |
|
613 |
} |
||
614 |
|||
615 |
// Norm of qs might be epsilon-different to 1, so M1.rotation might be epsilon-different to a rotation matrix. |
||
616 |
// It is then safer to re-normalized after converting M1.rotation to quaternion. |
||
617 |
✓✗ | 14139 |
quaternion::firstOrderNormalize(res_quat); |
618 |
✓✗✗✓ |
14139 |
assert(quaternion::isNormalized(res_quat,RealScalar(PINOCCHIO_DEFAULT_QUATERNION_NORM_TOLERANCE_VALUE))); |
619 |
14139 |
} |
|
620 |
|||
621 |
template <class Config_t, class Jacobian_t> |
||
622 |
22 |
static void integrateCoeffWiseJacobian_impl(const Eigen::MatrixBase<Config_t> & q, |
|
623 |
const Eigen::MatrixBase<Jacobian_t> & J) |
||
624 |
{ |
||
625 |
✓✗✓✗ ✓✗✓✗ |
22 |
assert(J.rows() == nq() && J.cols() == nv() && "J is not of the right dimension"); |
626 |
|||
627 |
typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(Config_t) ConfigPlainType; |
||
628 |
typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(Jacobian_t) JacobianPlainType; |
||
629 |
typedef typename ConfigPlainType::Scalar Scalar; |
||
630 |
|||
631 |
22 |
Jacobian_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t,J); |
|
632 |
✓✗ | 22 |
Jout.setZero(); |
633 |
|||
634 |
✓✗✓✗ |
22 |
ConstQuaternionMap_t quat_map(q.derived().template tail<4>().data()); |
635 |
✓✗✗✓ |
22 |
assert(quaternion::isNormalized(quat_map,RealScalar(PINOCCHIO_DEFAULT_QUATERNION_NORM_TOLERANCE_VALUE))); |
636 |
✓✗✓✗ ✓✗ |
22 |
Jout.template topLeftCorner<3,3>() = quat_map.toRotationMatrix(); |
637 |
// Jexp3(quat,Jout.template bottomRightCorner<4,3>()); |
||
638 |
|||
639 |
typedef Eigen::Matrix<Scalar,4,3,JacobianPlainType::Options|Eigen::RowMajor> Jacobian43; |
||
640 |
typedef SE3Tpl<Scalar,ConfigPlainType::Options> SE3; |
||
641 |
✓✗ | 22 |
Jacobian43 Jexp3QuatCoeffWise; |
642 |
|||
643 |
Scalar theta; |
||
644 |
✓✗ | 22 |
typename SE3::Vector3 v = quaternion::log3(quat_map,theta); |
645 |
✓✗ | 22 |
quaternion::Jexp3CoeffWise(v,Jexp3QuatCoeffWise); |
646 |
✓✗ | 22 |
typename SE3::Matrix3 Jlog; |
647 |
✓✗ | 22 |
Jlog3(theta,v,Jlog); |
648 |
|||
649 |
// std::cout << "Jexp3QuatCoeffWise\n" << Jexp3QuatCoeffWise << std::endl; |
||
650 |
// std::cout << "Jlog\n" << Jlog << std::endl; |
||
651 |
|||
652 |
// if(quat_map.w() >= 0.) // comes from the log3 for quaternions which may change the sign. |
||
653 |
✓✗✓✓ |
22 |
if(quat_map.coeffs()[3] >= Scalar(0)) // comes from the log3 for quaternions which may change the sign. |
654 |
✓✗✓✗ ✓✗✓✗ |
13 |
PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t,J).template bottomRightCorner<4,3>().noalias() = Jexp3QuatCoeffWise * Jlog; |
655 |
else |
||
656 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
9 |
PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t,J).template bottomRightCorner<4,3>().noalias() = -Jexp3QuatCoeffWise * Jlog; |
657 |
22 |
} |
|
658 |
|||
659 |
template <class Config_t, class Tangent_t, class JacobianOut_t> |
||
660 |
40 |
static void dIntegrate_dq_impl(const Eigen::MatrixBase<Config_t > & /*q*/, |
|
661 |
const Eigen::MatrixBase<Tangent_t> & v, |
||
662 |
const Eigen::MatrixBase<JacobianOut_t>& J, |
||
663 |
const AssignmentOperatorType op=SETTO) |
||
664 |
{ |
||
665 |
40 |
JacobianOut_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J); |
|
666 |
|||
667 |
✓✓✓✗ |
40 |
switch(op) |
668 |
{ |
||
669 |
38 |
case SETTO: |
|
670 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
38 |
Jout = exp6(MotionRef<const Tangent_t>(v.derived())).toDualActionMatrix().transpose(); |
671 |
38 |
break; |
|
672 |
1 |
case ADDTO: |
|
673 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
1 |
Jout += exp6(MotionRef<const Tangent_t>(v.derived())).toDualActionMatrix().transpose(); |
674 |
1 |
break; |
|
675 |
1 |
case RMTO: |
|
676 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
1 |
Jout -= exp6(MotionRef<const Tangent_t>(v.derived())).toDualActionMatrix().transpose(); |
677 |
1 |
break; |
|
678 |
default: |
||
679 |
assert(false && "Wrong Op requesed value"); |
||
680 |
break; |
||
681 |
} |
||
682 |
40 |
} |
|
683 |
|||
684 |
template <class Config_t, class Tangent_t, class JacobianOut_t> |
||
685 |
60 |
static void dIntegrate_dv_impl(const Eigen::MatrixBase<Config_t > & /*q*/, |
|
686 |
const Eigen::MatrixBase<Tangent_t> & v, |
||
687 |
const Eigen::MatrixBase<JacobianOut_t>& J, |
||
688 |
const AssignmentOperatorType op=SETTO) |
||
689 |
{ |
||
690 |
✓✓✓✗ |
60 |
switch(op) |
691 |
{ |
||
692 |
58 |
case SETTO: |
|
693 |
✓✗✓✗ |
58 |
Jexp6<SETTO>(MotionRef<const Tangent_t>(v.derived()), J.derived()); |
694 |
58 |
break; |
|
695 |
1 |
case ADDTO: |
|
696 |
✓✗✓✗ |
1 |
Jexp6<ADDTO>(MotionRef<const Tangent_t>(v.derived()), J.derived()); |
697 |
1 |
break; |
|
698 |
1 |
case RMTO: |
|
699 |
✓✗✓✗ |
1 |
Jexp6<RMTO>(MotionRef<const Tangent_t>(v.derived()), J.derived()); |
700 |
1 |
break; |
|
701 |
default: |
||
702 |
assert(false && "Wrong Op requesed value"); |
||
703 |
break; |
||
704 |
} |
||
705 |
60 |
} |
|
706 |
|||
707 |
template <class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t> |
||
708 |
1 |
void dIntegrateTransport_dq_impl(const Eigen::MatrixBase<Config_t > & /*q*/, |
|
709 |
const Eigen::MatrixBase<Tangent_t> & v, |
||
710 |
const Eigen::MatrixBase<JacobianIn_t> & Jin, |
||
711 |
const Eigen::MatrixBase<JacobianOut_t> & J_out) const |
||
712 |
{ |
||
713 |
1 |
JacobianOut_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J_out); |
|
714 |
✓✗ | 1 |
Eigen::Matrix<Scalar,6,6> Jtmp6; |
715 |
✗✓✗✗ ✓✗✗✓ ✗✗✓✗ ✗✓✗✗ ✗ |
1 |
Jtmp6 = exp6(MotionRef<const Tangent_t>(v.derived())).toDualActionMatrix().transpose(); |
716 |
|||
717 |
Jout.template topRows<3>().noalias() |
||
718 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
1 |
= Jtmp6.template topLeftCorner<3,3>() * Jin.template topRows<3>(); |
719 |
Jout.template topRows<3>().noalias() |
||
720 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
1 |
+= Jtmp6.template topRightCorner<3,3>() * Jin.template bottomRows<3>(); |
721 |
Jout.template bottomRows<3>().noalias() |
||
722 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
1 |
= Jtmp6.template bottomRightCorner<3,3>() * Jin.template bottomRows<3>(); |
723 |
1 |
} |
|
724 |
|||
725 |
template <class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t> |
||
726 |
1 |
void dIntegrateTransport_dv_impl(const Eigen::MatrixBase<Config_t > & /*q*/, |
|
727 |
const Eigen::MatrixBase<Tangent_t> & v, |
||
728 |
const Eigen::MatrixBase<JacobianIn_t> & Jin, |
||
729 |
const Eigen::MatrixBase<JacobianOut_t> & J_out) const |
||
730 |
{ |
||
731 |
1 |
JacobianOut_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J_out); |
|
732 |
✓✗ | 1 |
Eigen::Matrix<Scalar,6,6> Jtmp6; |
733 |
✗✓✗✗ ✓✗✗✗ |
1 |
Jexp6<SETTO>(MotionRef<const Tangent_t>(v.derived()), Jtmp6); |
734 |
|||
735 |
Jout.template topRows<3>().noalias() |
||
736 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
1 |
= Jtmp6.template topLeftCorner<3,3>() * Jin.template topRows<3>(); |
737 |
Jout.template topRows<3>().noalias() |
||
738 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
1 |
+= Jtmp6.template topRightCorner<3,3>() * Jin.template bottomRows<3>(); |
739 |
Jout.template bottomRows<3>().noalias() |
||
740 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
1 |
= Jtmp6.template bottomRightCorner<3,3>() * Jin.template bottomRows<3>(); |
741 |
1 |
} |
|
742 |
|||
743 |
|||
744 |
template <class Config_t, class Tangent_t, class Jacobian_t> |
||
745 |
1 |
void dIntegrateTransport_dq_impl(const Eigen::MatrixBase<Config_t > & /*q*/, |
|
746 |
const Eigen::MatrixBase<Tangent_t> & v, |
||
747 |
const Eigen::MatrixBase<Jacobian_t> & J_out) const |
||
748 |
{ |
||
749 |
1 |
Jacobian_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t,J_out); |
|
750 |
✓✗ | 1 |
Eigen::Matrix<Scalar,6,6> Jtmp6; |
751 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
1 |
Jtmp6 = exp6(MotionRef<const Tangent_t>(v.derived())).toDualActionMatrix().transpose(); |
752 |
|||
753 |
//Aliasing |
||
754 |
Jout.template topRows<3>() |
||
755 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
1 |
= Jtmp6.template topLeftCorner<3,3>() * Jout.template topRows<3>(); |
756 |
Jout.template topRows<3>().noalias() |
||
757 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
1 |
+= Jtmp6.template topRightCorner<3,3>() * Jout.template bottomRows<3>(); |
758 |
Jout.template bottomRows<3>() |
||
759 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
1 |
= Jtmp6.template bottomRightCorner<3,3>() * Jout.template bottomRows<3>(); |
760 |
1 |
} |
|
761 |
|||
762 |
template <class Config_t, class Tangent_t, class Jacobian_t> |
||
763 |
1 |
void dIntegrateTransport_dv_impl(const Eigen::MatrixBase<Config_t > & /*q*/, |
|
764 |
const Eigen::MatrixBase<Tangent_t> & v, |
||
765 |
const Eigen::MatrixBase<Jacobian_t> & J_out) const |
||
766 |
{ |
||
767 |
1 |
Jacobian_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t,J_out); |
|
768 |
✓✗ | 1 |
Eigen::Matrix<Scalar,6,6> Jtmp6; |
769 |
✓✗✓✗ |
1 |
Jexp6<SETTO>(MotionRef<const Tangent_t>(v.derived()), Jtmp6); |
770 |
|||
771 |
Jout.template topRows<3>() |
||
772 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
1 |
= Jtmp6.template topLeftCorner<3,3>() * Jout.template topRows<3>(); |
773 |
Jout.template topRows<3>().noalias() |
||
774 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
1 |
+= Jtmp6.template topRightCorner<3,3>() * Jout.template bottomRows<3>(); |
775 |
Jout.template bottomRows<3>() |
||
776 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
1 |
= Jtmp6.template bottomRightCorner<3,3>() * Jout.template bottomRows<3>(); |
777 |
1 |
} |
|
778 |
|||
779 |
template <class ConfigL_t, class ConfigR_t> |
||
780 |
2063 |
static Scalar squaredDistance_impl(const Eigen::MatrixBase<ConfigL_t> & q0, |
|
781 |
const Eigen::MatrixBase<ConfigR_t> & q1) |
||
782 |
{ |
||
783 |
✓✗ | 2063 |
TangentVector_t t; |
784 |
✓✗ | 2063 |
difference_impl(q0, q1, t); |
785 |
✓✗ | 2063 |
return t.squaredNorm(); |
786 |
} |
||
787 |
|||
788 |
template <class Config_t> |
||
789 |
2065 |
static void normalize_impl (const Eigen::MatrixBase<Config_t>& qout) |
|
790 |
{ |
||
791 |
2065 |
Config_t & qout_ = PINOCCHIO_EIGEN_CONST_CAST(Config_t,qout); |
|
792 |
✓✗ | 2065 |
qout_.template tail<4>().normalize(); |
793 |
2065 |
} |
|
794 |
|||
795 |
template <class Config_t> |
||
796 |
10211 |
static bool isNormalized_impl(const Eigen::MatrixBase<Config_t>& qin, |
|
797 |
const Scalar& prec) |
||
798 |
{ |
||
799 |
✓✗ | 10211 |
Scalar norm = Scalar(qin.template tail<4>().norm()); |
800 |
using std::abs; |
||
801 |
10211 |
return abs(norm - Scalar(1.0)) < prec; |
|
802 |
} |
||
803 |
|||
804 |
template <class Config_t> |
||
805 |
305 |
void random_impl (const Eigen::MatrixBase<Config_t>& qout) const |
|
806 |
{ |
||
807 |
✓✗ | 305 |
R3crossSO3_t().random(qout); |
808 |
305 |
} |
|
809 |
|||
810 |
template <class ConfigL_t, class ConfigR_t, class ConfigOut_t> |
||
811 |
6230 |
void randomConfiguration_impl(const Eigen::MatrixBase<ConfigL_t> & lower, |
|
812 |
const Eigen::MatrixBase<ConfigR_t> & upper, |
||
813 |
const Eigen::MatrixBase<ConfigOut_t> & qout) |
||
814 |
const |
||
815 |
{ |
||
816 |
✓✗ | 6230 |
R3crossSO3_t ().randomConfiguration(lower, upper, qout); |
817 |
6230 |
} |
|
818 |
|||
819 |
template <class ConfigL_t, class ConfigR_t> |
||
820 |
5 |
static bool isSameConfiguration_impl(const Eigen::MatrixBase<ConfigL_t> & q0, |
|
821 |
const Eigen::MatrixBase<ConfigR_t> & q1, |
||
822 |
const Scalar & prec) |
||
823 |
{ |
||
824 |
✓✗ | 5 |
return R3crossSO3_t().isSameConfiguration(q0, q1, prec); |
825 |
} |
||
826 |
}; // struct SpecialEuclideanOperationTpl<3> |
||
827 |
|||
828 |
} // namespace pinocchio |
||
829 |
|||
830 |
#endif // ifndef __pinocchio_multibody_liegroup_special_euclidean_operation_hpp__ |
| Generated by: GCOVR (Version 4.2) |