GCC Code Coverage Report

Directory: ./
File: include/pinocchio/multibody/liegroup/special-euclidean.hpp
Date: 2025-02-12 21:03:38
Exec Total Coverage
Lines: 391 408 95.8%
Branches: 584 1436 40.7%
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 15740 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/4
✓ Branch 1 taken 7879 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
 15740 const Scalar omega = v(2);
72 ✗ Scalar cv, sv;
73
0/2
✗ Branch 1 not taken.
✗ Branch 2 not taken.
 15740 SINCOS(omega, &sv, &cv);
74
4/10
✓ Branch 2 taken 7879 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 7879 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 7879 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 7879 times.
✗ Branch 12 not taken.
✗ Branch 14 not taken.
✗ Branch 15 not taken.
 15740 PINOCCHIO_EIGEN_CONST_CAST(Matrix2Like, R) << cv, -sv, sv, cv;
75 using internal::if_then_else;
76
77 {
78
3/8
✓ Branch 1 taken 7879 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7879 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 7879 times.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
 15740 typename PINOCCHIO_EIGEN_PLAIN_TYPE(Vector2Like) vcross(-v(1), v(0));
79
8/18
✓ Branch 1 taken 7879 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7879 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 7879 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 7879 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 7879 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 7879 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 7879 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 7879 times.
✗ Branch 23 not taken.
✗ Branch 25 not taken.
✗ Branch 26 not taken.
 15740 vcross -= -v(1) * R.col(0) + v(0) * R.col(1);
80
1/2
✓ Branch 1 taken 7879 times.
✗ Branch 2 not taken.
 15740 vcross /= omega;
81
0/2
✗ Branch 1 not taken.
✗ Branch 2 not taken.
 15740 Scalar omega_abs = math::fabs(omega);
82
1/12
✗ Branch 1 not taken.
✓ Branch 2 taken 169 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 12 not taken.
 15740 PINOCCHIO_EIGEN_CONST_CAST(Vector2Like, t).coeffRef(0) = if_then_else(
83
1/2
✓ Branch 1 taken 7710 times.
✗ Branch 2 not taken.
 15403 internal::GT, omega_abs, Scalar(1e-14), // TODO: change hard coded value
84
3/6
✓ Branch 1 taken 7879 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 169 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 169 times.
✗ Branch 8 not taken.
 15740 vcross.coeff(0), v.coeff(0));
85
86
1/12
✗ Branch 1 not taken.
✓ Branch 2 taken 169 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 12 not taken.
 15740 PINOCCHIO_EIGEN_CONST_CAST(Vector2Like, t).coeffRef(1) = if_then_else(
87
1/2
✓ Branch 1 taken 7710 times.
✗ Branch 2 not taken.
 15403 internal::GT, omega_abs, Scalar(1e-14), // TODO: change hard coded value
88
3/6
✓ Branch 1 taken 7879 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 169 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 169 times.
✗ Branch 8 not taken.
 15740 vcross.coeff(1), v.coeff(1));
89 }
90 15740 }
91
92 template<typename Matrix2Like, typename Vector2Like, typename Matrix3Like>
93 44 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 38 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 38 times.
✗ Branch 5 not taken.
 44 PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix3Like, M, 3, 3);
102 44 Matrix3Like & Mout = PINOCCHIO_EIGEN_CONST_CAST(Matrix3Like, M);
103 typedef typename Matrix3Like::Scalar Scalar;
104
105
4/8
✓ Branch 1 taken 38 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 38 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 38 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 38 times.
✗ Branch 11 not taken.
 44 typename PINOCCHIO_EIGEN_PLAIN_TYPE(Vector2Like) tinv((R.transpose() * t).reverse());
106
1/6
✓ Branch 1 taken 38 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
 44 tinv[0] *= Scalar(-1.);
107
3/4
✓ Branch 0 taken 36 times.
✓ Branch 1 taken 1 times.
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 44 switch (op)
108 {
109 42 case SETTO:
110
3/6
✓ Branch 1 taken 36 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 36 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 36 times.
✗ Branch 8 not taken.
 42 Mout.template topLeftCorner<2, 2>() = R.transpose();
111
2/4
✓ Branch 1 taken 36 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 36 times.
✗ Branch 5 not taken.
 42 Mout.template topRightCorner<2, 1>() = tinv;
112
2/4
✓ Branch 1 taken 36 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 36 times.
✗ Branch 5 not taken.
 42 Mout.template bottomLeftCorner<1, 2>().setZero();
113
1/6
✓ Branch 1 taken 36 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
 42 Mout(2, 2) = (Scalar)1;
114 42 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/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 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/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
 1 Mout(2, 2) -= (Scalar)1;
124 1 break;
125 ✗ default:
126 ✗ assert(false && "Wrong Op requesed value");
127 break;
128 }
129 44 }
130
131 template<typename Matrix2Like, typename Vector2Like, typename TangentVector>
132 15095 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 15095 TangentVector & vout = PINOCCHIO_EIGEN_CONST_CAST(TangentVector, v);
142
143 typedef typename Matrix2Like::Scalar Scalar1;
144
145
1/2
✓ Branch 1 taken 7548 times.
✗ Branch 2 not taken.
 15095 Scalar1 t = SO2_t::log(R);
146
0/2
✗ Branch 1 not taken.
✗ Branch 2 not taken.
 15095 const Scalar1 tabs = math::fabs(t);
147
0/2
✗ Branch 1 not taken.
✗ Branch 2 not taken.
 15095 const Scalar1 t2 = t * t;
148 ✗ Scalar1 st, ct;
149
0/2
✗ Branch 1 not taken.
✗ Branch 2 not taken.
 15095 SINCOS(tabs, &st, &ct);
150 ✗ Scalar1 alpha;
151
0/4
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
 30190 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/12
✓ Branch 1 taken 7548 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 16 not taken.
✗ Branch 17 not taken.
 15095 static_cast<Scalar>(tabs * st / (2 * (1 - ct))));
155
156
4/8
✓ Branch 1 taken 7548 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7548 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 7548 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 7548 times.
✗ Branch 11 not taken.
 15095 vout.template head<2>().noalias() = alpha * p;
157
2/12
✓ Branch 1 taken 7548 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7548 times.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 16 not taken.
✗ Branch 17 not taken.
 15095 vout(0) += t / 2 * p(1);
158
2/14
✓ Branch 1 taken 7548 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7548 times.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 16 not taken.
✗ Branch 17 not taken.
✗ Branch 19 not taken.
✗ Branch 20 not taken.
 15095 vout(1) += -t / 2 * p(0);
159
1/4
✓ Branch 1 taken 7548 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
 15095 vout(2) = t;
160 15095 }
161
162 template<typename Matrix2Like, typename Vector2Like, typename JacobianOutLike>
163 92 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 92 JacobianOutLike & Jout = PINOCCHIO_EIGEN_CONST_CAST(JacobianOutLike, J);
173
174 typedef typename Matrix2Like::Scalar Scalar1;
175
176
1/2
✓ Branch 1 taken 76 times.
✗ Branch 2 not taken.
 92 Scalar1 t = SO2_t::log(R);
177
0/2
✗ Branch 1 not taken.
✗ Branch 2 not taken.
 92 const Scalar1 tabs = math::fabs(t);
178 ✗ Scalar1 alpha, alpha_dot;
179
0/2
✗ Branch 1 not taken.
✗ Branch 2 not taken.
 92 Scalar1 t2 = t * t;
180 ✗ Scalar1 st, ct;
181
0/2
✗ Branch 1 not taken.
✗ Branch 2 not taken.
 92 SINCOS(t, &st, &ct);
182
0/8
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
 92 Scalar1 inv_2_1_ct = 0.5 / (1 - ct);
183
184
0/4
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
 184 alpha = internal::if_then_else(
185 ✗ internal::LT, tabs, Scalar(1e-4), static_cast<Scalar>(1 - t2 / 12),
186
1/4
✓ Branch 1 taken 76 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
 92 static_cast<Scalar>(t * st * inv_2_1_ct));
187
0/4
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
 184 alpha_dot = internal::if_then_else(
188 ✗ internal::LT, tabs, Scalar(1e-4), static_cast<Scalar>(-t / 6 - t2 * t / 180),
189
1/4
✓ Branch 1 taken 76 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
 92 static_cast<Scalar>((st - t) * inv_2_1_ct));
190
191
1/2
✓ Branch 1 taken 76 times.
✗ Branch 2 not taken.
 92 typename PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix2Like) V;
192
2/8
✓ Branch 1 taken 76 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 76 times.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
 92 V(0, 0) = V(1, 1) = alpha;
193
1/10
✓ Branch 1 taken 76 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
 92 V(1, 0) = -t / 2;
194
2/8
✓ Branch 1 taken 76 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 76 times.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
 92 V(0, 1) = -V(1, 0);
195
196
4/8
✓ Branch 1 taken 76 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 76 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 76 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 76 times.
✗ Branch 11 not taken.
 92 Jout.template topLeftCorner<2, 2>().noalias() = V * R;
197
4/24
✓ Branch 1 taken 76 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 76 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 76 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 76 times.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 16 not taken.
✗ Branch 17 not taken.
✗ Branch 19 not taken.
✗ Branch 20 not taken.
✗ Branch 22 not taken.
✗ Branch 23 not taken.
✗ Branch 25 not taken.
✗ Branch 26 not taken.
✗ Branch 28 not taken.
✗ Branch 29 not taken.
✗ Branch 31 not taken.
✗ Branch 32 not taken.
✗ Branch 34 not taken.
✗ Branch 35 not taken.
 92 Jout.template topRightCorner<2, 1>() << alpha_dot * p[0] + p[1] / 2,
198
3/8
✓ Branch 1 taken 76 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 76 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 76 times.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
 92 -p(0) / 2 + alpha_dot * p(1);
199
2/4
✓ Branch 1 taken 76 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 76 times.
✗ Branch 5 not taken.
 92 Jout.template bottomLeftCorner<1, 2>().setZero();
200
1/6
✓ Branch 1 taken 76 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
 92 Jout(2, 2) = 1;
201 92 }
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 9798 static Index nq()
208 {
209 9798 return NQ;
210 }
211 /// Get dimension of Lie Group tangent space
212 7757 static Index nv()
213 {
214 7757 return NV;
215 }
216
217 13 static ConfigVector_t neutral()
218 {
219 13 ConfigVector_t n;
220
4/16
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 13 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 13 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 13 times.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 16 not taken.
✗ Branch 17 not taken.
✗ Branch 19 not taken.
✗ Branch 20 not taken.
✗ Branch 22 not taken.
✗ Branch 23 not taken.
 13 n << Scalar(0), Scalar(0), Scalar(1), Scalar(0);
221 13 return n;
222 }
223
224 89 static std::string name()
225 {
226
1/2
✓ Branch 2 taken 89 times.
✗ Branch 3 not taken.
 89 return std::string("SE(2)");
227 }
228
229 template<class ConfigL_t, class ConfigR_t, class Tangent_t>
230 15095 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 7548 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7548 times.
✗ Branch 5 not taken.
 15095 Matrix2 R0, R1;
238
2/4
✓ Branch 1 taken 7548 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7548 times.
✗ Branch 5 not taken.
 15095 Vector2 t0, t1;
239
1/2
✓ Branch 1 taken 7548 times.
✗ Branch 2 not taken.
 15095 forwardKinematics(R0, t0, q0);
240
1/2
✓ Branch 1 taken 7548 times.
✗ Branch 2 not taken.
 15095 forwardKinematics(R1, t1, q1);
241 PINOCCHIO_COMPILER_DIAGNOSTIC_POP
242
3/6
✓ Branch 1 taken 7548 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7548 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 7548 times.
✗ Branch 8 not taken.
 15095 Matrix2 R(R0.transpose() * R1);
243
4/8
✓ Branch 1 taken 7548 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7548 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 7548 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 7548 times.
✗ Branch 11 not taken.
 15095 Vector2 t(R0.transpose() * (t1 - t0));
244
245
1/2
✓ Branch 1 taken 7548 times.
✗ Branch 2 not taken.
 15095 log(R, t, d);
246 15095 }
247
248 template<ArgumentPosition arg, class ConfigL_t, class ConfigR_t, class JacobianOut_t>
249 152 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 76 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 76 times.
✗ Branch 5 not taken.
 152 Matrix2 R0, R1;
257
2/4
✓ Branch 1 taken 76 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 76 times.
✗ Branch 5 not taken.
 152 Vector2 t0, t1;
258
1/2
✓ Branch 1 taken 76 times.
✗ Branch 2 not taken.
 152 forwardKinematics(R0, t0, q0);
259
1/2
✓ Branch 1 taken 76 times.
✗ Branch 2 not taken.
 152 forwardKinematics(R1, t1, q1);
260 PINOCCHIO_COMPILER_DIAGNOSTIC_POP
261
3/6
✓ Branch 1 taken 76 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 76 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 76 times.
✗ Branch 8 not taken.
 152 Matrix2 R(R0.transpose() * R1);
262
4/8
✓ Branch 1 taken 76 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 76 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 76 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 76 times.
✗ Branch 11 not taken.
 152 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 28 times.
✗ Branch 2 not taken.
 56 JacobianMatrix_t J1;
269
1/2
✓ Branch 1 taken 28 times.
✗ Branch 2 not taken.
 56 Jlog(R, t, J1);
270 PINOCCHIO_COMPILER_DIAGNOSTIC_POP
271
272 // pcross = [ y1-y0, - (x1 - x0) ]
273
5/14
✓ Branch 1 taken 28 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 28 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 28 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 28 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 28 times.
✗ Branch 14 not taken.
✗ Branch 16 not taken.
✗ Branch 17 not taken.
✗ Branch 19 not taken.
✗ Branch 20 not taken.
 56 Vector2 pcross(q1(1) - q0(1), q0(0) - q1(0));
274
275 56 JacobianOut_t & J0 = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, J);
276
5/10
✓ Branch 1 taken 28 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 28 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 28 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 28 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 28 times.
✗ Branch 14 not taken.
 56 J0.template topLeftCorner<2, 2>().noalias() = -R.transpose();
277
5/10
✓ Branch 1 taken 28 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 28 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 28 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 28 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 28 times.
✗ Branch 14 not taken.
 56 J0.template topRightCorner<2, 1>().noalias() = R1.transpose() * pcross;
278
2/4
✓ Branch 1 taken 28 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 28 times.
✗ Branch 5 not taken.
 56 J0.template bottomLeftCorner<1, 2>().setZero();
279
1/6
✓ Branch 1 taken 28 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
 56 J0(2, 2) = -1;
280
1/2
✓ Branch 1 taken 28 times.
✗ Branch 2 not taken.
 56 J0.applyOnTheLeft(J1);
281 }
282 else if (arg == ARG1)
283 {
284
1/2
✓ Branch 1 taken 48 times.
✗ Branch 2 not taken.
 96 Jlog(R, t, J);
285 }
286 152 }
287
288 template<class ConfigIn_t, class Velocity_t, class ConfigOut_t>
289 15499 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 15499 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 7754 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7754 times.
✗ Branch 5 not taken.
 15499 Matrix2 R0, R;
299
2/4
✓ Branch 1 taken 7754 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7754 times.
✗ Branch 5 not taken.
 15499 Vector2 t0, t;
300
1/2
✓ Branch 1 taken 7754 times.
✗ Branch 2 not taken.
 15499 forwardKinematics(R0, t0, q);
301
1/2
✓ Branch 1 taken 7754 times.
✗ Branch 2 not taken.
 15499 exp(v, R, t);
302 PINOCCHIO_COMPILER_DIAGNOSTIC_POP
303
304
5/10
✓ Branch 1 taken 7754 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7754 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 7754 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 7754 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 7754 times.
✗ Branch 14 not taken.
 15499 out.template head<2>().noalias() = R0 * t + t0;
305
5/10
✓ Branch 1 taken 7754 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7754 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 7754 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 7754 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 7754 times.
✗ Branch 14 not taken.
 15499 out.template tail<2>().noalias() = R0 * R.col(0);
306 15499 }
307
308 template<class Config_t, class Jacobian_t>
309 20 static void integrateCoeffWiseJacobian_impl(
310 const Eigen::MatrixBase<Config_t> & q, const Eigen::MatrixBase<Jacobian_t> & J)
311 {
312
2/4
✓ Branch 2 taken 20 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 20 times.
✗ Branch 7 not taken.
 20 assert(J.rows() == nq() && J.cols() == nv() && "J is not of the right dimension");
313
314 20 Jacobian_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t, J);
315
0/2
✗ Branch 1 not taken.
✗ Branch 2 not taken.
 20 Jout.setZero();
316
317
0/4
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
 20 const typename Config_t::Scalar &c_theta = q(2), &s_theta = q(3);
318
319
4/14
✗ Branch 1 not taken.
✓ Branch 2 taken 20 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 20 times.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✓ Branch 8 taken 20 times.
✗ Branch 9 not taken.
✗ Branch 10 not taken.
✓ Branch 11 taken 20 times.
✗ Branch 12 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
 20 Jout.template topLeftCorner<2, 2>() << c_theta, -s_theta, s_theta, c_theta;
320
2/8
✗ Branch 1 not taken.
✓ Branch 2 taken 20 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 20 times.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
 20 Jout.template bottomRightCorner<2, 1>() << -s_theta, c_theta;
321 20 }
322
323 template<class Config_t, class Tangent_t, class JacobianOut_t>
324 44 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 44 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 38 times.
✗ Branch 2 not taken.
 44 Matrix2 R;
335
1/2
✓ Branch 1 taken 38 times.
✗ Branch 2 not taken.
 44 Vector2 t;
336
1/2
✓ Branch 1 taken 38 times.
✗ Branch 2 not taken.
 44 exp(v, R, t);
337 PINOCCHIO_COMPILER_DIAGNOSTIC_POP
338
339
1/2
✓ Branch 1 taken 38 times.
✗ Branch 2 not taken.
 44 toInverseActionMatrix(R, t, Jout, op);
340 44 }
341
342 template<class Config_t, class Tangent_t, class JacobianOut_t>
343 64 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 64 JacobianOut_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, J);
350 // TODO sparse version
351
1/2
✓ Branch 1 taken 58 times.
✗ Branch 2 not taken.
 64 MotionTpl<Scalar, 0> nu;
352
8/22
✓ Branch 1 taken 58 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 58 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 58 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 58 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 58 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 58 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 58 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 58 times.
✗ Branch 23 not taken.
✗ Branch 25 not taken.
✗ Branch 26 not taken.
✗ Branch 28 not taken.
✗ Branch 29 not taken.
✗ Branch 31 not taken.
✗ Branch 32 not taken.
 64 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 58 times.
✗ Branch 2 not taken.
 64 Eigen::Matrix<Scalar, 6, 6> Jtmp6;
356
1/2
✓ Branch 1 taken 58 times.
✗ Branch 2 not taken.
 64 Jexp6(nu, Jtmp6);
357 PINOCCHIO_COMPILER_DIAGNOSTIC_POP
358
359
3/4
✓ Branch 0 taken 56 times.
✓ Branch 1 taken 1 times.
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 64 switch (op)
360 {
361 62 case SETTO:
362
5/10
✓ Branch 1 taken 56 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 56 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 56 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 56 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 56 times.
✗ Branch 14 not taken.
 124 Jout << Jtmp6.template topLeftCorner<2, 2>(), Jtmp6.template topRightCorner<2, 1>(),
363
3/6
✓ Branch 1 taken 56 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 56 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 56 times.
✗ Branch 8 not taken.
 124 Jtmp6.template bottomLeftCorner<1, 2>(), Jtmp6.template bottomRightCorner<1, 1>();
364 62 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 64 }
382
383 template<class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
384 166 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 166 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 86 times.
✗ Branch 2 not taken.
 166 Matrix2 R;
394
1/2
✓ Branch 1 taken 86 times.
✗ Branch 2 not taken.
 166 Vector2 t;
395
1/2
✓ Branch 1 taken 86 times.
✗ Branch 2 not taken.
 166 exp(v, R, t);
396 PINOCCHIO_COMPILER_DIAGNOSTIC_POP
397
398
4/8
✓ Branch 1 taken 86 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 86 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 86 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 86 times.
✗ Branch 11 not taken.
 166 Vector2 tinv = (R.transpose() * t).reverse();
399
1/6
✓ Branch 1 taken 86 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
 166 tinv[0] *= Scalar(-1.);
400
401
6/12
✓ Branch 1 taken 86 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 86 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 86 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 86 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 86 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 86 times.
✗ Branch 17 not taken.
 166 Jout.template topRows<2>().noalias() = R.transpose() * Jin.template topRows<2>();
402
5/10
✓ Branch 1 taken 86 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 86 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 86 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 86 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 86 times.
✗ Branch 14 not taken.
 166 Jout.template topRows<2>().noalias() += tinv * Jin.template bottomRows<1>();
403
3/6
✓ Branch 1 taken 86 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 86 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 86 times.
✗ Branch 8 not taken.
 166 Jout.template bottomRows<1>() = Jin.template bottomRows<1>();
404 166 }
405
406 template<class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
407 2 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 2 JacobianOut_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, J_out);
414
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 MotionTpl<Scalar, 0> nu;
415
8/22
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 2 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 2 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 2 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 2 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 2 times.
✗ Branch 23 not taken.
✗ Branch 25 not taken.
✗ Branch 26 not taken.
✗ Branch 28 not taken.
✗ Branch 29 not taken.
✗ Branch 31 not taken.
✗ Branch 32 not taken.
 2 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 2 times.
✗ Branch 2 not taken.
 2 Eigen::Matrix<Scalar, 6, 6> Jtmp6;
420
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 Jexp6(nu, Jtmp6);
421 PINOCCHIO_COMPILER_DIAGNOSTIC_POP
422
423
3/6
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
 2 Jout.template topRows<2>().noalias() =
424
3/6
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
 2 Jtmp6.template topLeftCorner<2, 2>() * Jin.template topRows<2>();
425
3/6
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
 2 Jout.template topRows<2>().noalias() +=
426
3/6
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
 2 Jtmp6.template topRightCorner<2, 1>() * Jin.template bottomRows<1>();
427
3/6
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
 2 Jout.template bottomRows<1>().noalias() =
428
3/6
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
 2 Jtmp6.template bottomLeftCorner<1, 2>() * Jin.template topRows<2>();
429
3/6
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
 2 Jout.template bottomRows<1>().noalias() +=
430
3/6
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
 2 Jtmp6.template bottomRightCorner<1, 1>() * Jin.template bottomRows<1>();
431 2 }
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/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 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/22
✓ 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.
✗ Branch 25 not taken.
✗ Branch 26 not taken.
✗ Branch 28 not taken.
✗ Branch 29 not taken.
✗ Branch 31 not taken.
✗ Branch 32 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 2054 static void normalize_impl(const Eigen::MatrixBase<Config_t> & qout)
483 {
484
1/2
✓ Branch 3 taken 1031 times.
✗ Branch 4 not taken.
 2054 pinocchio::normalize(qout.const_cast_derived().template tail<2>());
485 2054 }
486
487 template<class Config_t>
488 10209 static bool isNormalized_impl(const Eigen::MatrixBase<Config_t> & qin, const Scalar & prec)
489 {
490
1/2
✓ Branch 2 taken 5108 times.
✗ Branch 3 not taken.
 10209 const Scalar norm = Scalar(qin.template tail<2>().norm());
491 using std::abs;
492 10209 return abs(norm - Scalar(1.0)) < prec;
493 }
494
495 template<class Config_t>
496 262 static void random_impl(const Eigen::MatrixBase<Config_t> & qout)
497 {
498
1/2
✓ Branch 2 taken 135 times.
✗ Branch 3 not taken.
 262 R2crossSO2_t().random(qout);
499 262 }
500
501 template<class ConfigL_t, class ConfigR_t, class ConfigOut_t>
502 6141 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 3078 times.
✗ Branch 3 not taken.
 6141 R2crossSO2_t().randomConfiguration(lower, upper, qout);
508 6141 }
509
510 template<class ConfigL_t, class ConfigR_t>
511 5 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 4 times.
✗ Branch 3 not taken.
 5 return R2crossSO2_t().isSameConfiguration(q0, q1, prec);
517 }
518
519 protected:
520 template<typename Matrix2Like, typename Vector2Like, typename Vector4Like>
521 45554 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
3/6
✓ Branch 1 taken 455 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 22547 times.
✗ Branch 4 not taken.
✓ Branch 5 taken 455 times.
✗ Branch 6 not taken.
 45554 PINOCCHIO_EIGEN_CONST_CAST(Vector2Like, t) = q.template head<2>();
531
2/4
✓ Branch 1 taken 455 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 455 times.
✗ Branch 5 not taken.
 45554 const typename Vector4Like::Scalar &c_theta = q(2), &s_theta = q(3);
532
7/13
✓ Branch 2 taken 455 times.
✓ Branch 3 taken 22547 times.
✗ Branch 4 not taken.
✓ Branch 5 taken 455 times.
✓ Branch 6 taken 22547 times.
✗ Branch 7 not taken.
✓ Branch 8 taken 455 times.
✓ Branch 9 taken 22547 times.
✗ Branch 10 not taken.
✓ Branch 11 taken 455 times.
✗ Branch 12 not taken.
✗ Branch 14 not taken.
✗ Branch 15 not taken.
 45554 PINOCCHIO_EIGEN_CONST_CAST(Matrix2Like, R) << c_theta, -s_theta, s_theta, c_theta;
533 45554 }
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 10163 static Index nq()
572 {
573 10163 return NQ;
574 }
575 /// Get dimension of Lie Group tangent space
576 6983 static Index nv()
577 {
578 6983 return NV;
579 }
580
581 63 static ConfigVector_t neutral()
582 {
583 63 ConfigVector_t n;
584
3/5
✓ Branch 1 taken 1 times.
✓ Branch 2 taken 62 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 63 n.template head<6>().setZero();
585
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.
 63 n[6] = 1;
586 63 return n;
587 }
588
589 91 static std::string name()
590 {
591
1/2
✓ Branch 2 taken 91 times.
✗ Branch 3 not taken.
 91 return std::string("SE(3)");
592 }
593
594 template<class ConfigL_t, class ConfigR_t, class Tangent_t>
595 16145 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 8074 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 8074 times.
✗ Branch 7 not taken.
 16145 ConstQuaternionMap_t quat0(q0.derived().template tail<4>().data());
602
3/7
✓ Branch 1 taken 8074 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 8074 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✓ Branch 7 taken 165 times.
 16145 assert(quaternion::isNormalized(
603 quat0, RealScalar(PINOCCHIO_DEFAULT_QUATERNION_NORM_TOLERANCE_VALUE)));
604
2/4
✓ Branch 2 taken 8074 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 8074 times.
✗ Branch 7 not taken.
 16145 ConstQuaternionMap_t quat1(q1.derived().template tail<4>().data());
605
3/7
✓ Branch 1 taken 8074 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 8074 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✓ Branch 7 taken 165 times.
 16145 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 8074 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 8074 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 8074 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 8074 times.
✗ Branch 13 not taken.
 16145 const Vector3 dv_pre = q1.derived().template head<3>() - q0.derived().template head<3>();
610
2/4
✓ Branch 1 taken 8074 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8074 times.
✗ Branch 5 not taken.
 16145 const Vector3 dv = quat0.conjugate() * dv_pre;
611
2/4
✓ Branch 1 taken 8074 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8074 times.
✗ Branch 5 not taken.
 16145 PINOCCHIO_EIGEN_CONST_CAST(Tangent_t, d).noalias() =
612
4/8
✓ Branch 1 taken 8074 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8074 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 8074 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 8074 times.
✗ Branch 11 not taken.
 16474 log6(quat0.conjugate() * quat1, dv).toVector();
613 16145 }
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 256 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 128 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 128 times.
✗ Branch 7 not taken.
 256 ConstQuaternionMap_t quat0(q0.derived().template tail<4>().data());
626
2/7
✓ Branch 1 taken 128 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 128 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
 256 assert(quaternion::isNormalized(
627 quat0, RealScalar(PINOCCHIO_DEFAULT_QUATERNION_NORM_TOLERANCE_VALUE)));
628
2/4
✓ Branch 2 taken 128 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 128 times.
✗ Branch 7 not taken.
 256 ConstQuaternionMap_t quat1(q1.derived().template tail<4>().data());
629
2/7
✓ Branch 1 taken 128 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 128 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
 256 assert(quaternion::isNormalized(
630 quat1, RealScalar(PINOCCHIO_DEFAULT_QUATERNION_NORM_TOLERANCE_VALUE)));
631
632
4/8
✓ Branch 2 taken 128 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 128 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 128 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 128 times.
✗ Branch 13 not taken.
 256 const Vector3 dv_pre = q1.derived().template head<3>() - q0.derived().template head<3>();
633
2/4
✓ Branch 1 taken 128 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 128 times.
✗ Branch 5 not taken.
 256 const Vector3 trans = quat0.conjugate() * dv_pre;
634
635
2/4
✓ Branch 1 taken 128 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 128 times.
✗ Branch 5 not taken.
 256 const Quaternion_t quat_diff = quat0.conjugate() * quat1;
636
637
1/2
✓ Branch 1 taken 128 times.
✗ Branch 2 not taken.
 256 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 54 times.
✗ Branch 2 not taken.
 108 JacobianMatrix_t J1;
644
1/2
✓ Branch 1 taken 54 times.
✗ Branch 2 not taken.
 108 Jlog6(M, J1);
645 PINOCCHIO_COMPILER_DIAGNOSTIC_POP
646
647
2/4
✓ Branch 1 taken 54 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 54 times.
✗ Branch 5 not taken.
 108 const Vector3 p1_p0 = quat1.conjugate() * dv_pre;
648
649 108 JacobianOut_t & J0 = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, J);
650
5/10
✓ Branch 1 taken 54 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 54 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 54 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 54 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 54 times.
✗ Branch 14 not taken.
 108 J0.template bottomRightCorner<3, 3>() = J0.template topLeftCorner<3, 3>() =
651
2/4
✓ Branch 1 taken 54 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 54 times.
✗ Branch 5 not taken.
 108 -M.rotation().transpose();
652
3/6
✓ Branch 1 taken 54 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 54 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 54 times.
✗ Branch 8 not taken.
 108 J0.template topRightCorner<3, 3>().noalias() =
653
4/8
✓ Branch 1 taken 54 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 54 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 54 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 54 times.
✗ Branch 11 not taken.
 108 skew(p1_p0) * M.rotation().transpose(); // = R1.T * skew(q1_t - q0_t) * R0;
654
2/4
✓ Branch 1 taken 54 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 54 times.
✗ Branch 5 not taken.
 108 J0.template bottomLeftCorner<3, 3>().setZero();
655
1/2
✓ Branch 1 taken 54 times.
✗ Branch 2 not taken.
 108 J0.applyOnTheLeft(J1);
656 }
657 else if (arg == ARG1)
658 {
659
1/2
✓ Branch 1 taken 74 times.
✗ Branch 2 not taken.
 148 Jlog6(M, J);
660 }
661 256 }
662
663 template<class ConfigIn_t, class Velocity_t, class ConfigOut_t>
664 22853 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 22853 ConfigOut_t & out = PINOCCHIO_EIGEN_CONST_CAST(ConfigOut_t, qout);
670
2/4
✓ Branch 2 taken 11641 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 11641 times.
✗ Branch 6 not taken.
 22853 Quaternion_t const quat(q.derived().template tail<4>());
671
3/7
✓ Branch 1 taken 11641 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 11641 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✓ Branch 7 taken 329 times.
 22853 assert(quaternion::isNormalized(
672 quat, RealScalar(PINOCCHIO_DEFAULT_QUATERNION_NORM_TOLERANCE_VALUE)));
673
2/4
✓ Branch 1 taken 11641 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 11641 times.
✗ Branch 6 not taken.
 22853 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 11641 times.
✗ Branch 2 not taken.
 22853 Eigen::Matrix<Scalar, 7, 1, Options> expv;
684
1/2
✓ Branch 1 taken 11641 times.
✗ Branch 2 not taken.
 22853 quaternion::exp6(v, expv);
685
686
6/12
✓ Branch 2 taken 11641 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 11641 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 11641 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 11641 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 11641 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 11641 times.
✗ Branch 18 not taken.
 22853 out.template head<3>() = (quat * expv.template head<3>()) + q.derived().template head<3>();
687
688
2/4
✓ Branch 1 taken 11641 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 11641 times.
✗ Branch 6 not taken.
 22853 ConstQuaternionMap_t quat1(expv.template tail<4>().data());
689
2/4
✓ Branch 1 taken 11641 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 11641 times.
✗ Branch 5 not taken.
 22853 res_quat = quat * quat1;
690
691
1/2
✓ Branch 1 taken 11641 times.
✗ Branch 2 not taken.
 22853 const Scalar dot_product = res_quat.dot(quat);
692
2/2
✓ Branch 0 taken 46564 times.
✓ Branch 1 taken 11641 times.
 114265 for (Eigen::DenseIndex k = 0; k < 4; ++k)
693 {
694
2/4
✓ Branch 1 taken 1316 times.
✗ Branch 2 not taken.
✓ Branch 6 taken 1316 times.
✗ Branch 7 not taken.
 91412 res_quat.coeffs().coeffRef(k) = if_then_else(
695
2/4
✓ Branch 3 taken 46564 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1316 times.
✗ Branch 7 not taken.
 182824 internal::LT, dot_product, Scalar(0), static_cast<Scalar>(-res_quat.coeffs().coeff(k)),
696 91412 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 11641 times.
✗ Branch 2 not taken.
 22853 quaternion::firstOrderNormalize(res_quat);
703
3/7
✓ Branch 1 taken 11641 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 11641 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✓ Branch 7 taken 329 times.
 22853 assert(quaternion::isNormalized(
704 res_quat, RealScalar(PINOCCHIO_DEFAULT_QUATERNION_NORM_TOLERANCE_VALUE)));
705 22853 }
706
707 template<class Config_t, class Jacobian_t>
708 22 static void integrateCoeffWiseJacobian_impl(
709 const Eigen::MatrixBase<Config_t> & q, const Eigen::MatrixBase<Jacobian_t> & J)
710 {
711
2/4
✓ Branch 2 taken 22 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 22 times.
✗ Branch 7 not taken.
 22 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 22 Jacobian_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t, J);
718
1/2
✓ Branch 1 taken 22 times.
✗ Branch 2 not taken.
 22 Jout.setZero();
719
720
2/4
✓ Branch 2 taken 22 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 22 times.
✗ Branch 7 not taken.
 22 ConstQuaternionMap_t quat_map(q.derived().template tail<4>().data());
721
2/4
✓ Branch 1 taken 22 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 22 times.
 22 assert(quaternion::isNormalized(
722 quat_map, RealScalar(PINOCCHIO_DEFAULT_QUATERNION_NORM_TOLERANCE_VALUE)));
723
3/6
✓ Branch 1 taken 22 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 22 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 22 times.
✗ Branch 8 not taken.
 22 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 22 times.
✗ Branch 2 not taken.
 22 Jacobian43 Jexp3QuatCoeffWise;
731
732 Scalar theta;
733
1/2
✓ Branch 1 taken 22 times.
✗ Branch 2 not taken.
 22 typename SE3::Vector3 v = quaternion::log3(quat_map, theta);
734
1/2
✓ Branch 1 taken 22 times.
✗ Branch 2 not taken.
 22 quaternion::Jexp3CoeffWise(v, Jexp3QuatCoeffWise);
735
1/2
✓ Branch 1 taken 22 times.
✗ Branch 2 not taken.
 22 typename SE3::Matrix3 Jlog;
736
1/2
✓ Branch 1 taken 22 times.
✗ Branch 2 not taken.
 22 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
3/4
✓ Branch 2 taken 22 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 11 times.
✓ Branch 5 taken 11 times.
 22 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 11 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 11 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 11 times.
✗ Branch 9 not taken.
 11 PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t, J).template bottomRightCorner<4, 3>().noalias() =
747
1/2
✓ Branch 1 taken 11 times.
✗ Branch 2 not taken.
 22 Jexp3QuatCoeffWise * Jlog;
748 else
749
5/10
✓ Branch 1 taken 11 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 11 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 11 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 11 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 11 times.
✗ Branch 15 not taken.
 11 PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t, J).template bottomRightCorner<4, 3>().noalias() =
750 -Jexp3QuatCoeffWise * Jlog;
751 22 }
752
753 template<class Config_t, class Tangent_t, class JacobianOut_t>
754 53 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 53 JacobianOut_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, J);
761
762
3/4
✓ Branch 0 taken 43 times.
✓ Branch 1 taken 1 times.
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 53 switch (op)
763 {
764 51 case SETTO:
765
4/8
✓ Branch 3 taken 43 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 43 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 43 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 43 times.
✗ Branch 13 not taken.
 51 Jout = exp6(MotionRef<const Tangent_t>(v.derived())).toDualActionMatrix().transpose();
766 51 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 53 }
778
779 template<class Config_t, class Tangent_t, class JacobianOut_t>
780 73 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 63 times.
✓ Branch 1 taken 1 times.
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 73 switch (op)
787 {
788 71 case SETTO:
789
1/2
✓ Branch 4 taken 63 times.
✗ Branch 5 not taken.
 71 Jexp6<SETTO>(MotionRef<const Tangent_t>(v.derived()), J.derived());
790 71 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 73 }
802
803 template<class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
804 167 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 167 JacobianOut_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, J_out);
811
1/2
✓ Branch 1 taken 87 times.
✗ Branch 2 not taken.
 167 Eigen::Matrix<Scalar, 6, 6> Jtmp6;
812
5/10
✓ Branch 2 taken 87 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 87 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 87 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 87 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 87 times.
✗ Branch 15 not taken.
 167 Jtmp6 = exp6(MotionRef<const Tangent_t>(v.derived())).toDualActionMatrix().transpose();
813
814
3/6
✓ Branch 1 taken 87 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 87 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 87 times.
✗ Branch 8 not taken.
 167 Jout.template topRows<3>().noalias() =
815
3/6
✓ Branch 1 taken 87 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 87 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 87 times.
✗ Branch 8 not taken.
 167 Jtmp6.template topLeftCorner<3, 3>() * Jin.template topRows<3>();
816
3/6
✓ Branch 1 taken 87 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 87 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 87 times.
✗ Branch 8 not taken.
 167 Jout.template topRows<3>().noalias() +=
817
3/6
✓ Branch 1 taken 87 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 87 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 87 times.
✗ Branch 8 not taken.
 167 Jtmp6.template topRightCorner<3, 3>() * Jin.template bottomRows<3>();
818
3/6
✓ Branch 1 taken 87 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 87 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 87 times.
✗ Branch 8 not taken.
 167 Jout.template bottomRows<3>().noalias() =
819
3/6
✓ Branch 1 taken 87 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 87 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 87 times.
✗ Branch 8 not taken.
 167 Jtmp6.template bottomRightCorner<3, 3>() * Jin.template bottomRows<3>();
820 167 }
821
822 template<class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
823 3 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 3 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 3 times.
✗ Branch 2 not taken.
 3 Eigen::Matrix<Scalar, 6, 6> Jtmp6;
833
2/4
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
 3 Jexp6<SETTO>(MotionRef<const Tangent_t>(v.derived()), Jtmp6);
834 PINOCCHIO_COMPILER_DIAGNOSTIC_POP
835
836
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.
 3 Jout.template topRows<3>().noalias() =
837
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.
 3 Jtmp6.template topLeftCorner<3, 3>() * Jin.template topRows<3>();
838
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.
 3 Jout.template topRows<3>().noalias() +=
839
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.
 3 Jtmp6.template topRightCorner<3, 3>() * Jin.template bottomRows<3>();
840
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.
 3 Jout.template bottomRows<3>().noalias() =
841
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.
 3 Jtmp6.template bottomRightCorner<3, 3>() * Jin.template bottomRows<3>();
842 3 }
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 2067 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 1037 times.
✗ Branch 2 not taken.
 2067 TangentVector_t t;
891
1/2
✓ Branch 1 taken 1037 times.
✗ Branch 2 not taken.
 2067 difference_impl(q0, q1, t);
892 PINOCCHIO_COMPILER_DIAGNOSTIC_POP
893
1/2
✓ Branch 1 taken 1037 times.
✗ Branch 2 not taken.
 4134 return t.squaredNorm();
894 }
895
896 template<class Config_t>
897 2067 static void normalize_impl(const Eigen::MatrixBase<Config_t> & qout)
898 {
899
1/2
✓ Branch 3 taken 1042 times.
✗ Branch 4 not taken.
 2067 pinocchio::normalize(qout.const_cast_derived().template tail<4>());
900 2067 }
901
902 template<class Config_t>
903 10221 static bool isNormalized_impl(const Eigen::MatrixBase<Config_t> & qin, const Scalar & prec)
904 {
905
1/2
✓ Branch 2 taken 5120 times.
✗ Branch 3 not taken.
 10221 Scalar norm = Scalar(qin.template tail<4>().norm());
906 using std::abs;
907 10221 return abs(norm - Scalar(1.0)) < prec;
908 }
909
910 template<class Config_t>
911 350 static void random_impl(const Eigen::MatrixBase<Config_t> & qout)
912 {
913
1/2
✓ Branch 2 taken 179 times.
✗ Branch 3 not taken.
 350 R3crossSO3_t().random(qout);
914 350 }
915
916 template<class ConfigL_t, class ConfigR_t, class ConfigOut_t>
917 8564 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 5500 times.
✗ Branch 3 not taken.
 8564 R3crossSO3_t().randomConfiguration(lower, upper, qout);
923 8564 }
924
925 template<class ConfigL_t, class ConfigR_t>
926 9 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 7 times.
✗ Branch 3 not taken.
 9 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