Directory: | ./ |
---|---|
File: | include/pinocchio/spatial/explog-quaternion.hpp |
Date: | 2024-08-27 18:20:05 |
Exec | Total | Coverage | |
---|---|---|---|
Lines: | 78 | 97 | 80.4% |
Branches: | 95 | 221 | 43.0% |
Line | Branch | Exec | Source |
---|---|---|---|
1 | // | ||
2 | // Copyright (c) 2018-2021 CNRS INRIA | ||
3 | // | ||
4 | |||
5 | #ifndef __pinocchio_spatial_explog_quaternion_hpp__ | ||
6 | #define __pinocchio_spatial_explog_quaternion_hpp__ | ||
7 | |||
8 | #include "pinocchio/math/quaternion.hpp" | ||
9 | #include "pinocchio/spatial/explog.hpp" | ||
10 | #include "pinocchio/utils/static-if.hpp" | ||
11 | |||
12 | namespace pinocchio | ||
13 | { | ||
14 | namespace quaternion | ||
15 | { | ||
16 | |||
17 | /// | ||
18 | /// \brief Exp: so3 -> SO3 (quaternion) | ||
19 | /// | ||
20 | /// \returns the integral of the velocity vector as a quaternion. | ||
21 | /// | ||
22 | /// \param[in] v The angular velocity vector. | ||
23 | /// \param[out] qout The quaternion where the result is stored. | ||
24 | /// | ||
25 | template<typename Vector3Like, typename QuaternionLike> | ||
26 | void | ||
27 | 6176 | exp3(const Eigen::MatrixBase<Vector3Like> & v, Eigen::QuaternionBase<QuaternionLike> & quat_out) | |
28 | { | ||
29 | EIGEN_STATIC_ASSERT_VECTOR_ONLY(Vector3Like); | ||
30 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 3088 times.
|
6176 | assert(v.size() == 3); |
31 | |||
32 | typedef typename Vector3Like::Scalar Scalar; | ||
33 | enum | ||
34 | { | ||
35 | Options = PINOCCHIO_EIGEN_PLAIN_TYPE(typename QuaternionLike::Coefficients)::Options | ||
36 | }; | ||
37 | typedef Eigen::Quaternion<typename QuaternionLike::Scalar, Options> QuaternionPlain; | ||
38 | 6176 | const Scalar eps = Eigen::NumTraits<Scalar>::epsilon(); | |
39 | |||
40 |
1/2✓ Branch 1 taken 3088 times.
✗ Branch 2 not taken.
|
6176 | const Scalar t2 = v.squaredNorm(); |
41 | 6176 | const Scalar t = math::sqrt(t2 + eps * eps); | |
42 | |||
43 |
3/6✓ Branch 0 taken 21 times.
✓ Branch 1 taken 3067 times.
✓ Branch 3 taken 21 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
|
6176 | static const Scalar ts_prec = |
44 |
1/2✓ Branch 1 taken 21 times.
✗ Branch 2 not taken.
|
42 | TaylorSeriesExpansion<Scalar>::template precision<3>(); // Precision for the Taylor series |
45 | // expansion. | ||
46 | |||
47 |
2/4✓ Branch 1 taken 3088 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3088 times.
✗ Branch 5 not taken.
|
6176 | Eigen::AngleAxis<Scalar> aa(t, v / t); |
48 |
1/2✓ Branch 1 taken 3088 times.
✗ Branch 2 not taken.
|
6176 | QuaternionPlain quat_then(aa); |
49 | |||
50 | // order 4 Taylor expansion in theta / (order 2 in t2) | ||
51 |
1/2✓ Branch 1 taken 3088 times.
✗ Branch 2 not taken.
|
6176 | QuaternionPlain quat_else; |
52 | 6176 | const Scalar t2_2 = t2 / 4; // theta/2 squared | |
53 |
2/4✓ Branch 1 taken 3088 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3088 times.
✗ Branch 5 not taken.
|
6176 | quat_else.vec() = |
54 |
1/2✓ Branch 1 taken 3088 times.
✗ Branch 2 not taken.
|
6176 | Scalar(0.5) * (Scalar(1) - t2_2 / Scalar(6) + t2_2 * t2_2 / Scalar(120)) * v; |
55 |
1/2✓ Branch 1 taken 3088 times.
✗ Branch 2 not taken.
|
6176 | quat_else.w() = Scalar(1) - t2_2 / 2 + t2_2 * t2_2 / 24; |
56 | |||
57 | using ::pinocchio::internal::if_then_else; | ||
58 |
2/2✓ Branch 0 taken 12352 times.
✓ Branch 1 taken 3088 times.
|
30880 | for (Eigen::DenseIndex k = 0; k < 4; ++k) |
59 | { | ||
60 |
2/4✓ Branch 1 taken 12352 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2496 times.
✗ Branch 5 not taken.
|
24704 | quat_out.coeffs().coeffRef(k) = if_then_else( |
61 |
2/4✓ Branch 1 taken 10740 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10740 times.
✗ Branch 5 not taken.
|
24704 | ::pinocchio::internal::GT, t2, ts_prec, quat_then.coeffs().coeffRef(k), |
62 |
1/2✓ Branch 2 taken 10740 times.
✗ Branch 3 not taken.
|
24704 | quat_else.coeffs().coeffRef(k)); |
63 | } | ||
64 | 6176 | } | |
65 | |||
66 | /// \brief Exp: so3 -> SO3 (quaternion) | ||
67 | /// | ||
68 | /// \returns the integral of the velocity vector as a quaternion. | ||
69 | /// | ||
70 | /// \param[in] v The angular velocity vector. | ||
71 | /// | ||
72 | template<typename Vector3Like> | ||
73 | Eigen:: | ||
74 | Quaternion<typename Vector3Like::Scalar, PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like)::Options> | ||
75 | exp3(const Eigen::MatrixBase<Vector3Like> & v) | ||
76 | { | ||
77 | typedef Eigen::Quaternion< | ||
78 | typename Vector3Like::Scalar, PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like)::Options> | ||
79 | ReturnType; | ||
80 | ReturnType res; | ||
81 | exp3(v, res); | ||
82 | return res; | ||
83 | } | ||
84 | |||
85 | /// \brief The se3 -> SE3 exponential map, using quaternions to represent the output rotation. | ||
86 | /// | ||
87 | /// \returns the integral of the twist motion over unit time. | ||
88 | /// | ||
89 | /// \param[in] motion the spatial motion. | ||
90 | /// \param[out] q the output transform in \f$\mathbb{R}^3 x S^3\f$. | ||
91 | template<typename MotionDerived, typename Config_t> | ||
92 | 5331 | void exp6(const MotionDense<MotionDerived> & motion, Eigen::MatrixBase<Config_t> & qout) | |
93 | { | ||
94 | enum | ||
95 | { | ||
96 | Options = PINOCCHIO_EIGEN_PLAIN_TYPE(Config_t)::Options | ||
97 | }; | ||
98 | typedef typename Config_t::Scalar Scalar; | ||
99 | typedef typename MotionDerived::Vector3 Vector3; | ||
100 | typedef Eigen::Quaternion<Scalar, Options> Quaternion_t; | ||
101 | 5331 | const Scalar eps = Eigen::NumTraits<Scalar>::epsilon(); | |
102 | |||
103 |
1/2✓ Branch 1 taken 2867 times.
✗ Branch 2 not taken.
|
5331 | const typename MotionDerived::ConstAngularType & w = motion.angular(); |
104 |
1/2✓ Branch 1 taken 2867 times.
✗ Branch 2 not taken.
|
5331 | const typename MotionDerived::ConstLinearType & v = motion.linear(); |
105 | |||
106 |
1/2✓ Branch 1 taken 2867 times.
✗ Branch 2 not taken.
|
5331 | const Scalar t2 = w.squaredNorm() + eps * eps; |
107 | 5331 | const Scalar t = math::sqrt(t2); | |
108 | |||
109 | Scalar ct, st; | ||
110 | 5331 | SINCOS(t, &st, &ct); | |
111 | |||
112 | 5331 | const Scalar inv_t2 = Scalar(1) / t2; | |
113 | 5331 | const Scalar ts_prec = | |
114 |
1/2✓ Branch 1 taken 2867 times.
✗ Branch 2 not taken.
|
5331 | TaylorSeriesExpansion<Scalar>::template precision<3>(); // Taylor expansion precision |
115 | |||
116 | using ::pinocchio::internal::if_then_else; | ||
117 | using ::pinocchio::internal::LT; | ||
118 | |||
119 | 10662 | const Scalar alpha_wxv = if_then_else( | |
120 | LT, t, ts_prec, | ||
121 | ✗ | Scalar(0.5) - t2 / Scalar(24), // then: use Taylor expansion | |
122 |
1/2✓ Branch 1 taken 2867 times.
✗ Branch 2 not taken.
|
5331 | (Scalar(1) - ct) * inv_t2 // else |
123 | ); | ||
124 | |||
125 | 10662 | const Scalar alpha_w2 = if_then_else( | |
126 |
1/2✓ Branch 1 taken 2867 times.
✗ Branch 2 not taken.
|
5331 | LT, t, ts_prec, Scalar(1) / Scalar(6) - t2 / Scalar(120), (t - st) * inv_t2 / t); |
127 | |||
128 | // linear part | ||
129 |
5/10✓ Branch 1 taken 2464 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 403 times.
✗ Branch 4 not taken.
✓ Branch 5 taken 2464 times.
✗ Branch 6 not taken.
✓ Branch 7 taken 403 times.
✗ Branch 8 not taken.
✓ Branch 9 taken 2464 times.
✗ Branch 10 not taken.
|
5331 | Eigen::Map<Vector3> trans_(qout.derived().template head<3>().data()); |
130 |
9/18✓ Branch 1 taken 2867 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2867 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2867 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 2867 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 2867 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 2867 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 2867 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 2867 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 2867 times.
✗ Branch 26 not taken.
|
5331 | trans_.noalias() = v + alpha_wxv * w.cross(v) + alpha_w2 * w.cross(w.cross(v)); |
131 | |||
132 | // quaternion part | ||
133 | typedef Eigen::Map<Quaternion_t> QuaternionMap_t; | ||
134 |
2/4✓ Branch 2 taken 2867 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 2867 times.
✗ Branch 7 not taken.
|
5331 | QuaternionMap_t quat_(qout.derived().template tail<4>().data()); |
135 |
1/2✓ Branch 1 taken 2867 times.
✗ Branch 2 not taken.
|
5331 | exp3(w, quat_); |
136 | 5331 | } | |
137 | |||
138 | /// \brief The se3 -> SE3 exponential map, using quaternions to represent the output rotation. | ||
139 | /// | ||
140 | /// \returns the integral of the twist motion over unit time. | ||
141 | /// | ||
142 | /// \param[in] motion the spatial motion. | ||
143 | template<typename MotionDerived> | ||
144 | Eigen::Matrix< | ||
145 | typename MotionDerived::Scalar, | ||
146 | 7, | ||
147 | 1, | ||
148 | PINOCCHIO_EIGEN_PLAIN_TYPE(typename MotionDerived::Vector3)::Options> | ||
149 | ✗ | exp6(const MotionDense<MotionDerived> & motion) | |
150 | { | ||
151 | typedef typename MotionDerived::Scalar Scalar; | ||
152 | enum | ||
153 | { | ||
154 | Options = PINOCCHIO_EIGEN_PLAIN_TYPE(typename MotionDerived::Vector3)::Options | ||
155 | }; | ||
156 | typedef Eigen::Matrix<Scalar, 7, 1, Options> ReturnType; | ||
157 | |||
158 | ✗ | ReturnType qout; | |
159 | ✗ | exp6(motion, qout); | |
160 | ✗ | return qout; | |
161 | } | ||
162 | |||
163 | /// \brief The se3 -> SE3 exponential map, using quaternions to represent the output rotation. | ||
164 | /// | ||
165 | /// \returns the integral of the spatial velocity over unit time. | ||
166 | /// | ||
167 | /// \param[in] vec6 the vector representing the spatial velocity. | ||
168 | /// \param[out] qout the output transform in R^3 x S^3. | ||
169 | template<typename Vector6Like, typename Config_t> | ||
170 | 5331 | void exp6(const Eigen::MatrixBase<Vector6Like> & vec6, Eigen::MatrixBase<Config_t> & qout) | |
171 | { | ||
172 |
2/5✓ Branch 2 taken 2867 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 403 times.
✗ Branch 6 not taken.
|
5331 | MotionRef<const Vector6Like> nu(vec6.derived()); |
173 |
1/2✓ Branch 1 taken 2867 times.
✗ Branch 2 not taken.
|
5331 | ::pinocchio::quaternion::exp6(nu, qout); |
174 | 5331 | } | |
175 | |||
176 | /// \brief The se3 -> SE3 exponential map, using quaternions to represent the output rotation. | ||
177 | /// | ||
178 | /// \returns the integral of the spatial velocity over unit time. | ||
179 | /// | ||
180 | /// \param[in] vec6 the vector representing the spatial velocity. | ||
181 | template<typename Vector6Like> | ||
182 | Eigen:: | ||
183 | Matrix<typename Vector6Like::Scalar, 7, 1, PINOCCHIO_EIGEN_PLAIN_TYPE(Vector6Like)::Options> | ||
184 | ✗ | exp6(const Eigen::MatrixBase<Vector6Like> & vec6) | |
185 | { | ||
186 | typedef typename Vector6Like::Scalar Scalar; | ||
187 | enum | ||
188 | { | ||
189 | Options = PINOCCHIO_EIGEN_PLAIN_TYPE(Vector6Like)::Options | ||
190 | }; | ||
191 | typedef Eigen::Matrix<Scalar, 7, 1, Options> ReturnType; | ||
192 | |||
193 | ✗ | ReturnType qout; | |
194 | ✗ | ::pinocchio::quaternion::exp6(vec6, qout); | |
195 | ✗ | return qout; | |
196 | } | ||
197 | |||
198 | /// \brief Same as \ref log3 but with a unit quaternion as input. | ||
199 | /// | ||
200 | /// \param[in] quat the unit quaternion. | ||
201 | /// \param[out] theta the angle value (resuling from compurations). | ||
202 | /// | ||
203 | /// \return The angular velocity vector associated to the rotation matrix. | ||
204 | /// | ||
205 | template<typename QuaternionLike> | ||
206 | Eigen::Matrix< | ||
207 | typename QuaternionLike::Scalar, | ||
208 | 3, | ||
209 | 1, | ||
210 | PINOCCHIO_EIGEN_PLAIN_TYPE(typename QuaternionLike::Vector3)::Options> | ||
211 | 522 | log3( | |
212 | const Eigen::QuaternionBase<QuaternionLike> & quat, typename QuaternionLike::Scalar & theta) | ||
213 | { | ||
214 | typedef typename QuaternionLike::Scalar Scalar; | ||
215 | enum | ||
216 | { | ||
217 | Options = PINOCCHIO_EIGEN_PLAIN_TYPE(typename QuaternionLike::Vector3)::Options | ||
218 | }; | ||
219 | typedef Eigen::Matrix<Scalar, 3, 1, Options> Vector3; | ||
220 | |||
221 |
1/2✓ Branch 1 taken 261 times.
✗ Branch 2 not taken.
|
522 | Vector3 res; |
222 |
2/4✓ Branch 1 taken 261 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 261 times.
✗ Branch 5 not taken.
|
522 | const Scalar norm_squared = quat.vec().squaredNorm(); |
223 | |||
224 | static const Scalar eps = Eigen::NumTraits<Scalar>::epsilon(); | ||
225 |
4/8✓ Branch 0 taken 2 times.
✓ Branch 1 taken 259 times.
✓ Branch 3 taken 2 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 2 times.
✗ Branch 7 not taken.
✗ Branch 9 not taken.
✗ Branch 10 not taken.
|
522 | static const Scalar ts_prec = TaylorSeriesExpansion<Scalar>::template precision<2>(); |
226 | 522 | const Scalar norm = math::sqrt(norm_squared + eps * eps); | |
227 | |||
228 | using ::pinocchio::internal::GE; | ||
229 | using ::pinocchio::internal::if_then_else; | ||
230 | using ::pinocchio::internal::LT; | ||
231 | |||
232 |
2/4✓ Branch 1 taken 261 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 261 times.
✗ Branch 5 not taken.
|
522 | const Scalar pos_neg = if_then_else(GE, quat.w(), Scalar(0), Scalar(+1), Scalar(-1)); |
233 | |||
234 |
1/2✓ Branch 1 taken 261 times.
✗ Branch 2 not taken.
|
522 | Eigen::Quaternion<Scalar, Options> quat_pos; |
235 |
2/4✓ Branch 1 taken 261 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 261 times.
✗ Branch 5 not taken.
|
522 | quat_pos.w() = pos_neg * quat.w(); |
236 |
4/8✓ Branch 1 taken 261 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 261 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 261 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 261 times.
✗ Branch 11 not taken.
|
522 | quat_pos.vec() = pos_neg * quat.vec(); |
237 | |||
238 |
2/4✓ Branch 1 taken 261 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 261 times.
✗ Branch 5 not taken.
|
522 | const Scalar theta_2 = math::atan2(norm, quat_pos.w()); // in [0,pi] |
239 |
1/2✓ Branch 1 taken 261 times.
✗ Branch 2 not taken.
|
522 | const Scalar y_x = norm / quat_pos.w(); // nonnegative |
240 |
2/4✓ Branch 1 taken 261 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 261 times.
✗ Branch 5 not taken.
|
522 | const Scalar y_x_sq = norm_squared / (quat_pos.w() * quat_pos.w()); |
241 | |||
242 | 1044 | theta = if_then_else( | |
243 | ✗ | LT, norm_squared, ts_prec, Scalar(2.) * (Scalar(1) - y_x_sq / Scalar(3)) * y_x, | |
244 |
1/2✓ Branch 1 taken 261 times.
✗ Branch 2 not taken.
|
522 | Scalar(2.) * theta_2); |
245 | |||
246 | 522 | const Scalar th2_2 = theta * theta / Scalar(4); | |
247 | 1044 | const Scalar inv_sinc = if_then_else( | |
248 | LT, norm_squared, ts_prec, | ||
249 | ✗ | Scalar(2) * (Scalar(1) + th2_2 / Scalar(6) + Scalar(7) / Scalar(360) * th2_2 * th2_2), | |
250 |
1/2✓ Branch 2 taken 261 times.
✗ Branch 3 not taken.
|
522 | theta / math::sin(theta_2)); |
251 | |||
252 |
2/2✓ Branch 0 taken 783 times.
✓ Branch 1 taken 261 times.
|
2088 | for (Eigen::DenseIndex k = 0; k < 3; ++k) |
253 | { | ||
254 | // res[k] = if_then_else(LT, norm_squared, ts_prec, | ||
255 | // Scalar(2) * (Scalar(1) + y_x_sq / Scalar(6) - y_x_sq*y_x_sq / | ||
256 | // Scalar(9)) * pos_neg * quat.vec()[k], inv_sinc * pos_neg * | ||
257 | // quat.vec()[k]); | ||
258 |
3/6✓ Branch 1 taken 783 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 783 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 783 times.
✗ Branch 8 not taken.
|
1566 | res[k] = inv_sinc * quat_pos.vec()[k]; |
259 | } | ||
260 | 1044 | return res; | |
261 | } | ||
262 | |||
263 | /// | ||
264 | /// \brief Log: SO3 -> so3. | ||
265 | /// | ||
266 | /// Pseudo-inverse of log from \f$ SO3 -> { v \in so3, ||v|| \le pi } \f$. | ||
267 | /// | ||
268 | /// \param[in] quat The unit quaternion representing a certain rotation. | ||
269 | /// | ||
270 | /// \return The angular velocity vector associated to the quaternion. | ||
271 | /// | ||
272 | template<typename QuaternionLike> | ||
273 | Eigen::Matrix< | ||
274 | typename QuaternionLike::Scalar, | ||
275 | 3, | ||
276 | 1, | ||
277 | PINOCCHIO_EIGEN_PLAIN_TYPE(typename QuaternionLike::Vector3)::Options> | ||
278 | 130 | log3(const Eigen::QuaternionBase<QuaternionLike> & quat) | |
279 | { | ||
280 | typename QuaternionLike::Scalar theta; | ||
281 |
1/2✓ Branch 2 taken 130 times.
✗ Branch 3 not taken.
|
260 | return log3(quat.derived(), theta); |
282 | } | ||
283 | |||
284 | /// | ||
285 | /// \brief Derivative of \f$ q = \exp{\mathbf{v} + \delta\mathbf{v}} \f$ where \f$ | ||
286 | /// \delta\mathbf{v} \f$ | ||
287 | /// is a small perturbation of \f$ \mathbf{v} \f$ at identity. | ||
288 | /// | ||
289 | /// \returns The Jacobian of the quaternion components variation. | ||
290 | /// | ||
291 | template<typename Vector3Like, typename Matrix43Like> | ||
292 | 1 | void Jexp3CoeffWise( | |
293 | const Eigen::MatrixBase<Vector3Like> & v, const Eigen::MatrixBase<Matrix43Like> & Jexp) | ||
294 | { | ||
295 | // EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix43Like,4,3); | ||
296 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
1 | assert(Jexp.rows() == 4 && Jexp.cols() == 3 && "Jexp does have the right size."); |
297 | 1 | Matrix43Like & Jout = PINOCCHIO_EIGEN_CONST_CAST(Matrix43Like, Jexp); | |
298 | |||
299 | typedef typename Vector3Like::Scalar Scalar; | ||
300 | |||
301 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | const Scalar n2 = v.squaredNorm(); |
302 | 1 | const Scalar n = math::sqrt(n2); | |
303 | 1 | const Scalar theta = Scalar(0.5) * n; | |
304 | 1 | const Scalar theta2 = Scalar(0.25) * n2; | |
305 | |||
306 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
1 | if (n2 > math::sqrt(Eigen::NumTraits<Scalar>::epsilon())) |
307 | { | ||
308 | Scalar c, s; | ||
309 | 1 | SINCOS(theta, &s, &c); | |
310 |
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 | Jout.template topRows<3>().noalias() = |
311 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
1 | ((Scalar(0.5) / n2) * (c - 2 * s / n)) * v * v.transpose(); |
312 |
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 | Jout.template topRows<3>().diagonal().array() += s / n; |
313 |
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 bottomRows<1>().noalias() = -s / (2 * n) * v.transpose(); |
314 | } | ||
315 | else | ||
316 | { | ||
317 | ✗ | Jout.template topRows<3>().noalias() = | |
318 | ✗ | (-Scalar(1) / Scalar(12) + n2 / Scalar(480)) * v * v.transpose(); | |
319 | ✗ | Jout.template topRows<3>().diagonal().array() += Scalar(0.5) * (1 - theta2 / 6); | |
320 | ✗ | Jout.template bottomRows<1>().noalias() = | |
321 | ✗ | (Scalar(-0.25) * (Scalar(1) - theta2 / 6)) * v.transpose(); | |
322 | } | ||
323 | 1 | } | |
324 | |||
325 | /// | ||
326 | /// \brief Computes the Jacobian of log3 operator for a unit quaternion. | ||
327 | /// | ||
328 | /// \param[in] quat A unit quaternion representing the input rotation. | ||
329 | /// \param[out] Jlog The resulting Jacobian of the log operator. | ||
330 | /// | ||
331 | template<typename QuaternionLike, typename Matrix3Like> | ||
332 | ✗ | void Jlog3( | |
333 | const Eigen::QuaternionBase<QuaternionLike> & quat, | ||
334 | const Eigen::MatrixBase<Matrix3Like> & Jlog) | ||
335 | { | ||
336 | typedef typename QuaternionLike::Scalar Scalar; | ||
337 | typedef Eigen::Matrix< | ||
338 | Scalar, 3, 1, PINOCCHIO_EIGEN_PLAIN_TYPE(typename QuaternionLike::Coefficients)::Options> | ||
339 | Vector3; | ||
340 | |||
341 | Scalar t; | ||
342 | ✗ | Vector3 w(log3(quat, t)); | |
343 | ✗ | pinocchio::Jlog3(t, w, PINOCCHIO_EIGEN_CONST_CAST(Matrix3Like, Jlog)); | |
344 | } | ||
345 | } // namespace quaternion | ||
346 | } // namespace pinocchio | ||
347 | |||
348 | #endif // ifndef __pinocchio_spatial_explog_quaternion_hpp__ | ||
349 |