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