Directory: | ./ |
---|---|
File: | include/pinocchio/spatial/explog.hpp |
Date: | 2024-08-27 18:20:05 |
Exec | Total | Coverage | |
---|---|---|---|
Lines: | 182 | 228 | 79.8% |
Branches: | 251 | 598 | 42.0% |
Line | Branch | Exec | Source |
---|---|---|---|
1 | // | ||
2 | // Copyright (c) 2015-2023 CNRS INRIA | ||
3 | // Copyright (c) 2015 Wandercraft, 86 rue de Paris 91400 Orsay, France. | ||
4 | // | ||
5 | |||
6 | #ifndef __pinocchio_spatial_explog_hpp__ | ||
7 | #define __pinocchio_spatial_explog_hpp__ | ||
8 | |||
9 | #include "pinocchio/fwd.hpp" | ||
10 | #include "pinocchio/utils/static-if.hpp" | ||
11 | #include "pinocchio/math/fwd.hpp" | ||
12 | #include "pinocchio/math/sincos.hpp" | ||
13 | #include "pinocchio/math/taylor-expansion.hpp" | ||
14 | #include "pinocchio/spatial/motion.hpp" | ||
15 | #include "pinocchio/spatial/skew.hpp" | ||
16 | #include "pinocchio/spatial/se3.hpp" | ||
17 | |||
18 | #include <Eigen/Geometry> | ||
19 | |||
20 | #include "pinocchio/spatial/log.hpp" | ||
21 | |||
22 | namespace pinocchio | ||
23 | { | ||
24 | /// \brief Exp: so3 -> SO3. | ||
25 | /// | ||
26 | /// Return the integral of the input angular velocity during time 1. | ||
27 | /// | ||
28 | /// \param[in] v The angular velocity vector. | ||
29 | /// | ||
30 | /// \return The rotational matrix associated to the integration of the angular velocity during | ||
31 | /// time 1. | ||
32 | /// | ||
33 | template<typename Vector3Like> | ||
34 | typename Eigen:: | ||
35 | Matrix<typename Vector3Like::Scalar, 3, 3, PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like)::Options> | ||
36 | 13 | exp3(const Eigen::MatrixBase<Vector3Like> & v) | |
37 | { | ||
38 |
2/4✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 13 times.
✗ Branch 5 not taken.
|
13 | PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE(Vector3Like, v, 3, 1); |
39 | |||
40 | typedef typename Vector3Like::Scalar Scalar; | ||
41 | typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like) Vector3LikePlain; | ||
42 | typedef Eigen::Matrix<Scalar, 3, 3, Vector3LikePlain::Options> Matrix3; | ||
43 | const static Scalar eps = Eigen::NumTraits<Scalar>::epsilon(); | ||
44 | |||
45 |
1/2✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
|
13 | const Scalar t2 = v.squaredNorm() + eps * eps; |
46 | |||
47 | 13 | const Scalar t = math::sqrt(t2); | |
48 | Scalar ct, st; | ||
49 | 13 | SINCOS(t, &st, &ct); | |
50 | |||
51 | 26 | const Scalar alpha_vxvx = internal::if_then_else( | |
52 |
1/2✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
|
13 | internal::GT, t, TaylorSeriesExpansion<Scalar>::template precision<3>(), |
53 |
1/2✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
|
13 | static_cast<Scalar>((1 - ct) / t2), static_cast<Scalar>(Scalar(1) / Scalar(2) - t2 / 24)); |
54 | 26 | const Scalar alpha_vx = internal::if_then_else( | |
55 |
1/2✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
|
13 | internal::GT, t, TaylorSeriesExpansion<Scalar>::template precision<3>(), |
56 |
1/2✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
|
13 | static_cast<Scalar>((st) / t), static_cast<Scalar>(Scalar(1) - t2 / 6)); |
57 |
4/8✓ 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.
|
13 | Matrix3 res(alpha_vxvx * v * v.transpose()); |
58 |
2/4✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 13 times.
✗ Branch 5 not taken.
|
13 | res.coeffRef(0, 1) -= alpha_vx * v[2]; |
59 |
2/4✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 13 times.
✗ Branch 5 not taken.
|
13 | res.coeffRef(1, 0) += alpha_vx * v[2]; |
60 |
2/4✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 13 times.
✗ Branch 5 not taken.
|
13 | res.coeffRef(0, 2) += alpha_vx * v[1]; |
61 |
2/4✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 13 times.
✗ Branch 5 not taken.
|
13 | res.coeffRef(2, 0) -= alpha_vx * v[1]; |
62 |
2/4✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 13 times.
✗ Branch 5 not taken.
|
13 | res.coeffRef(1, 2) -= alpha_vx * v[0]; |
63 |
2/4✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 13 times.
✗ Branch 5 not taken.
|
13 | res.coeffRef(2, 1) += alpha_vx * v[0]; |
64 | |||
65 | 26 | ct = internal::if_then_else( | |
66 |
1/2✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
|
13 | internal::GT, t, TaylorSeriesExpansion<Scalar>::template precision<3>(), ct, |
67 |
1/2✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
|
13 | static_cast<Scalar>(Scalar(1) - t2 / 2)); |
68 |
3/6✓ 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.
|
13 | res.diagonal().array() += ct; |
69 | |||
70 | 26 | return res; | |
71 | } | ||
72 | |||
73 | /// \brief Same as \ref log3 | ||
74 | /// | ||
75 | /// \param[in] R the rotation matrix. | ||
76 | /// \param[out] theta the angle value. | ||
77 | /// | ||
78 | /// \return The angular velocity vector associated to the rotation matrix. | ||
79 | /// | ||
80 | template<typename Matrix3Like> | ||
81 | Eigen:: | ||
82 | Matrix<typename Matrix3Like::Scalar, 3, 1, PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix3Like)::Options> | ||
83 | 1393 | log3(const Eigen::MatrixBase<Matrix3Like> & R, typename Matrix3Like::Scalar & theta) | |
84 | { | ||
85 | typedef typename Matrix3Like::Scalar Scalar; | ||
86 | typedef Eigen::Matrix<Scalar, 3, 1, PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix3Like)::Options> Vector3; | ||
87 | 1393 | Vector3 res; | |
88 | 1393 | log3_impl<Scalar>::run(R, theta, res); | |
89 | 1393 | return res; | |
90 | } | ||
91 | |||
92 | /// \brief Log: SO(3)-> so(3). | ||
93 | /// | ||
94 | /// Pseudo-inverse of log from \f$ SO3 -> { v \in so3, ||v|| \le pi } \f$. | ||
95 | /// | ||
96 | /// \param[in] R The rotation matrix. | ||
97 | /// | ||
98 | /// \return The angular velocity vector associated to the rotation matrix. | ||
99 | /// | ||
100 | template<typename Matrix3Like> | ||
101 | Eigen:: | ||
102 | Matrix<typename Matrix3Like::Scalar, 3, 1, PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix3Like)::Options> | ||
103 | ✗ | log3(const Eigen::MatrixBase<Matrix3Like> & R) | |
104 | { | ||
105 | typename Matrix3Like::Scalar theta; | ||
106 | ✗ | return log3(R.derived(), theta); | |
107 | } | ||
108 | |||
109 | /// | ||
110 | /// \brief Derivative of \f$ \exp{r} \f$ | ||
111 | /// \f[ | ||
112 | /// \frac{\sin{||r||}}{||r||} I_3 | ||
113 | /// - \frac{1-\cos{||r||}}{||r||^2} \left[ r \right]_x | ||
114 | /// + \frac{1}{||r||^2} (1-\frac{\sin{||r||}}{||r||}) r r^T | ||
115 | /// \f] | ||
116 | /// | ||
117 | template<AssignmentOperatorType op, typename Vector3Like, typename Matrix3Like> | ||
118 | 78 | void Jexp3(const Eigen::MatrixBase<Vector3Like> & r, const Eigen::MatrixBase<Matrix3Like> & Jexp) | |
119 | { | ||
120 |
2/4✓ Branch 1 taken 39 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 39 times.
✗ Branch 5 not taken.
|
78 | PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE(Vector3Like, r, 3, 1); |
121 |
2/4✓ Branch 1 taken 39 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 39 times.
✗ Branch 5 not taken.
|
78 | PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix3Like, Jexp, 3, 3); |
122 | |||
123 | 78 | Matrix3Like & Jout = PINOCCHIO_EIGEN_CONST_CAST(Matrix3Like, Jexp); | |
124 | typedef typename Matrix3Like::Scalar Scalar; | ||
125 | |||
126 |
1/2✓ Branch 1 taken 39 times.
✗ Branch 2 not taken.
|
78 | const Scalar n2 = r.squaredNorm(); |
127 | 78 | const Scalar n = math::sqrt(n2); | |
128 | 78 | const Scalar n_inv = Scalar(1) / n; | |
129 | 78 | const Scalar n2_inv = n_inv * n_inv; | |
130 | Scalar cn, sn; | ||
131 | 78 | SINCOS(n, &sn, &cn); | |
132 | |||
133 | 156 | const Scalar a = internal::if_then_else( | |
134 |
1/2✓ Branch 1 taken 39 times.
✗ Branch 2 not taken.
|
78 | internal::LT, n, TaylorSeriesExpansion<Scalar>::template precision<3>(), |
135 |
1/2✓ Branch 1 taken 39 times.
✗ Branch 2 not taken.
|
78 | static_cast<Scalar>(Scalar(1) - n2 / Scalar(6)), static_cast<Scalar>(sn * n_inv)); |
136 | 156 | const Scalar b = internal::if_then_else( | |
137 |
1/2✓ Branch 1 taken 39 times.
✗ Branch 2 not taken.
|
78 | internal::LT, n, TaylorSeriesExpansion<Scalar>::template precision<3>(), |
138 | ✗ | static_cast<Scalar>(-Scalar(1) / Scalar(2) - n2 / Scalar(24)), | |
139 |
1/2✓ Branch 1 taken 39 times.
✗ Branch 2 not taken.
|
78 | static_cast<Scalar>(-(1 - cn) * n2_inv)); |
140 | 156 | const Scalar c = internal::if_then_else( | |
141 |
1/2✓ Branch 1 taken 39 times.
✗ Branch 2 not taken.
|
78 | internal::LT, n, TaylorSeriesExpansion<Scalar>::template precision<3>(), |
142 | ✗ | static_cast<Scalar>(Scalar(1) / Scalar(6) - n2 / Scalar(120)), | |
143 |
1/2✓ Branch 1 taken 39 times.
✗ Branch 2 not taken.
|
78 | static_cast<Scalar>(n2_inv * (1 - a))); |
144 | |||
145 | switch (op) | ||
146 | { | ||
147 | case SETTO: | ||
148 |
2/4✓ Branch 1 taken 37 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 37 times.
✗ Branch 5 not taken.
|
74 | Jout.diagonal().setConstant(a); |
149 |
2/4✓ Branch 1 taken 37 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 37 times.
✗ Branch 5 not taken.
|
74 | Jout(0, 1) = -b * r[2]; |
150 |
2/4✓ Branch 1 taken 37 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 37 times.
✗ Branch 5 not taken.
|
74 | Jout(1, 0) = -Jout(0, 1); |
151 |
2/4✓ Branch 1 taken 37 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 37 times.
✗ Branch 5 not taken.
|
74 | Jout(0, 2) = b * r[1]; |
152 |
2/4✓ Branch 1 taken 37 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 37 times.
✗ Branch 5 not taken.
|
74 | Jout(2, 0) = -Jout(0, 2); |
153 |
2/4✓ Branch 1 taken 37 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 37 times.
✗ Branch 5 not taken.
|
74 | Jout(1, 2) = -b * r[0]; |
154 |
2/4✓ Branch 1 taken 37 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 37 times.
✗ Branch 5 not taken.
|
74 | Jout(2, 1) = -Jout(1, 2); |
155 |
5/10✓ Branch 1 taken 37 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 37 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 37 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 37 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 37 times.
✗ Branch 14 not taken.
|
74 | Jout.noalias() += c * r * r.transpose(); |
156 | 74 | break; | |
157 | case ADDTO: | ||
158 |
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.
|
2 | Jout.diagonal().array() += a; |
159 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | Jout(0, 1) += -b * r[2]; |
160 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | Jout(1, 0) += b * r[2]; |
161 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | Jout(0, 2) += b * r[1]; |
162 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | Jout(2, 0) += -b * r[1]; |
163 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | Jout(1, 2) += -b * r[0]; |
164 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | Jout(2, 1) += b * r[0]; |
165 |
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.
|
2 | Jout.noalias() += c * r * r.transpose(); |
166 | 2 | break; | |
167 | case RMTO: | ||
168 |
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.
|
2 | Jout.diagonal().array() -= a; |
169 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | Jout(0, 1) -= -b * r[2]; |
170 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | Jout(1, 0) -= b * r[2]; |
171 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | Jout(0, 2) -= b * r[1]; |
172 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | Jout(2, 0) -= -b * r[1]; |
173 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | Jout(1, 2) -= -b * r[0]; |
174 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | Jout(2, 1) -= b * r[0]; |
175 |
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.
|
2 | Jout.noalias() -= c * r * r.transpose(); |
176 | 2 | break; | |
177 | default: | ||
178 | assert(false && "Wrong Op requesed value"); | ||
179 | break; | ||
180 | } | ||
181 | 78 | } | |
182 | |||
183 | /// | ||
184 | /// \brief Derivative of \f$ \exp{r} \f$ | ||
185 | /// \f[ | ||
186 | /// \frac{\sin{||r||}}{||r||} I_3 | ||
187 | /// - \frac{1-\cos{||r||}}{||r||^2} \left[ r \right]_x | ||
188 | /// + \frac{1}{||r||^2} (1-\frac{\sin{||r||}}{||r||}) r r^T | ||
189 | /// \f] | ||
190 | /// | ||
191 | template<typename Vector3Like, typename Matrix3Like> | ||
192 | ✗ | void Jexp3(const Eigen::MatrixBase<Vector3Like> & r, const Eigen::MatrixBase<Matrix3Like> & Jexp) | |
193 | { | ||
194 | ✗ | Jexp3<SETTO>(r, Jexp); | |
195 | } | ||
196 | |||
197 | /** \brief Derivative of log3 | ||
198 | * | ||
199 | * This function is the right derivative of @ref log3, that is, for \f$R \in | ||
200 | * SO(3)\f$ and \f$\omega t in \mathfrak{so}(3)\f$, it provides the linear | ||
201 | * approximation: | ||
202 | * | ||
203 | * \f[ | ||
204 | * \log_3(R \oplus \omega t) = \log_3(R \exp_3(\omega t)) \approx \log_3(R) + \text{Jlog3}(R) | ||
205 | * \omega t \f] | ||
206 | * | ||
207 | * \param[in] theta the angle value. | ||
208 | * \param[in] log the output of log3. | ||
209 | * \param[out] Jlog the jacobian | ||
210 | * | ||
211 | * Equivalently, \f$\text{Jlog3}\f$ is the right Jacobian of \f$\log_3\f$: | ||
212 | * | ||
213 | * \f[ | ||
214 | * \text{Jlog3}(R) = \frac{\partial \log_3(R)}{\partial R} | ||
215 | * \f] | ||
216 | * | ||
217 | * Note that this is the right Jacobian: \f$\text{Jlog3}(R) : T_{R} SO(3) \to T_{\log_6(R)} | ||
218 | * \mathfrak{so}(3)\f$. (By convention, calculations in Pinocchio always perform right | ||
219 | * differentiation, i.e., Jacobians are in local coordinates (also known as body coordinates), | ||
220 | * unless otherwise specified.) | ||
221 | * | ||
222 | * If we denote by \f$\theta = \log_3(R)\f$ and \f$\log = \log_3(R, | ||
223 | * \theta)\f$, then \f$\text{Jlog} = \text{Jlog}_3(R)\f$ can be calculated as: | ||
224 | * | ||
225 | * \f[ | ||
226 | * \begin{align*} | ||
227 | * \text{Jlog} & = \frac{\theta \sin(\theta)}{2 (1 - \cos(\theta))} I_3 | ||
228 | * + \frac{1}{2} \widehat{\log} | ||
229 | * + \left(\frac{1}{\theta^2} - \frac{\sin(\theta)}{2\theta(1-\cos(\theta))}\right) | ||
230 | * \log \log^T \\ & = I_3 | ||
231 | * + \frac{1}{2} \widehat{\log} | ||
232 | * + \left(\frac{1}{\theta^2} - \frac{1 + \cos \theta}{2 \theta \sin \theta}\right) | ||
233 | * \widehat{\log}^2 \end{align*} \f] | ||
234 | * | ||
235 | * where \f$\widehat{v}\f$ denotes the skew-symmetric matrix obtained from the 3D vector \f$v\f$. | ||
236 | * | ||
237 | * \note The inputs must be such that \f$ \theta = \Vert \log \Vert \f$. | ||
238 | */ | ||
239 | template<typename Scalar, typename Vector3Like, typename Matrix3Like> | ||
240 | 227 | void Jlog3( | |
241 | const Scalar & theta, | ||
242 | const Eigen::MatrixBase<Vector3Like> & log, | ||
243 | const Eigen::MatrixBase<Matrix3Like> & Jlog) | ||
244 | { | ||
245 | 227 | Jlog3_impl<Scalar>::run(theta, log, PINOCCHIO_EIGEN_CONST_CAST(Matrix3Like, Jlog)); | |
246 | 227 | } | |
247 | |||
248 | /** \brief Derivative of log3 | ||
249 | * | ||
250 | * \param[in] R the rotation matrix. | ||
251 | * \param[out] Jlog the jacobian | ||
252 | * | ||
253 | * Equivalent to | ||
254 | * \code | ||
255 | * double theta; | ||
256 | * Vector3 log = pinocchio::log3 (R, theta); | ||
257 | * pinocchio::Jlog3 (theta, log, Jlog); | ||
258 | * \endcode | ||
259 | */ | ||
260 | template<typename Matrix3Like1, typename Matrix3Like2> | ||
261 | void | ||
262 | 12 | Jlog3(const Eigen::MatrixBase<Matrix3Like1> & R, const Eigen::MatrixBase<Matrix3Like2> & Jlog) | |
263 | { | ||
264 | typedef typename Matrix3Like1::Scalar Scalar; | ||
265 | typedef Eigen::Matrix<Scalar, 3, 1, PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix3Like1)::Options> Vector3; | ||
266 | |||
267 | Scalar t; | ||
268 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
12 | Vector3 w(log3(R, t)); |
269 |
1/2✓ Branch 2 taken 6 times.
✗ Branch 3 not taken.
|
12 | Jlog3(t, w, PINOCCHIO_EIGEN_CONST_CAST(Matrix3Like2, Jlog)); |
270 | 12 | } | |
271 | |||
272 | template<typename Scalar, typename Vector3Like1, typename Vector3Like2, typename Matrix3Like> | ||
273 | ✗ | void Hlog3( | |
274 | const Scalar & theta, | ||
275 | const Eigen::MatrixBase<Vector3Like1> & log, | ||
276 | const Eigen::MatrixBase<Vector3Like2> & v, | ||
277 | const Eigen::MatrixBase<Matrix3Like> & vt_Hlog) | ||
278 | { | ||
279 | typedef Eigen::Matrix<Scalar, 3, 1, PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix3Like)::Options> Vector3; | ||
280 | ✗ | Matrix3Like & vt_Hlog_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix3Like, vt_Hlog); | |
281 | |||
282 | // theta = (log^T * log)^.5 | ||
283 | // dt/dl = .5 * 2 * log^T * (log^T * log)^-.5 | ||
284 | // = log^T / theta | ||
285 | // dt_dl = log / theta | ||
286 | Scalar ctheta, stheta; | ||
287 | ✗ | SINCOS(theta, &stheta, &ctheta); | |
288 | |||
289 | ✗ | Scalar denom = .5 / (1 - ctheta), a = theta * stheta * denom, | |
290 | ✗ | da_dt = (stheta - theta) * denom, // da / dtheta | |
291 | ✗ | b = (1 - a) / (theta * theta), | |
292 | // db_dt = - (2 * (1 - a) / theta + da_dt ) / theta**2; // db / dtheta | ||
293 | ✗ | db_dt = -(2 / theta - (theta + stheta) * denom) / (theta * theta); // db / dtheta | |
294 | |||
295 | // Compute dl_dv_v = Jlog * v | ||
296 | // Jlog = a I3 + .5 [ log ] + b * log * log^T | ||
297 | // if v == log, then Jlog * v == v | ||
298 | ✗ | Vector3 dl_dv_v(a * v + .5 * log.cross(v) + b * log * log.transpose() * v); | |
299 | |||
300 | ✗ | Scalar dt_dv_v = log.dot(dl_dv_v) / theta; | |
301 | |||
302 | // Derivative of b * log * log^T | ||
303 | ✗ | vt_Hlog_.noalias() = db_dt * dt_dv_v * log * log.transpose(); | |
304 | ✗ | vt_Hlog_.noalias() += b * dl_dv_v * log.transpose(); | |
305 | ✗ | vt_Hlog_.noalias() += b * log * dl_dv_v.transpose(); | |
306 | // Derivative of .5 * [ log ] | ||
307 | ✗ | addSkew(.5 * dl_dv_v, vt_Hlog_); | |
308 | // Derivative of a * I3 | ||
309 | ✗ | vt_Hlog_.diagonal().array() += da_dt * dt_dv_v; | |
310 | } | ||
311 | |||
312 | /** \brief Second order derivative of log3 | ||
313 | * | ||
314 | * This computes \f$ v^T H_{log} \f$. | ||
315 | * | ||
316 | * \param[in] R the rotation matrix. | ||
317 | * \param[in] v the 3D vector. | ||
318 | * \param[out] vt_Hlog the product of the Hessian with the input vector | ||
319 | */ | ||
320 | template<typename Matrix3Like1, typename Vector3Like, typename Matrix3Like2> | ||
321 | ✗ | void Hlog3( | |
322 | const Eigen::MatrixBase<Matrix3Like1> & R, | ||
323 | const Eigen::MatrixBase<Vector3Like> & v, | ||
324 | const Eigen::MatrixBase<Matrix3Like2> & vt_Hlog) | ||
325 | { | ||
326 | typedef typename Matrix3Like1::Scalar Scalar; | ||
327 | typedef Eigen::Matrix<Scalar, 3, 1, PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix3Like1)::Options> Vector3; | ||
328 | |||
329 | Scalar t; | ||
330 | ✗ | Vector3 w(log3(R, t)); | |
331 | ✗ | Hlog3(t, w, v, PINOCCHIO_EIGEN_CONST_CAST(Matrix3Like2, vt_Hlog)); | |
332 | } | ||
333 | |||
334 | /// | ||
335 | /// \brief Exp: se3 -> SE3. | ||
336 | /// | ||
337 | /// Return the integral of the input twist during time 1. | ||
338 | /// | ||
339 | /// \param[in] nu The input twist. | ||
340 | /// | ||
341 | /// \return The rigid transformation associated to the integration of the twist during time 1. | ||
342 | /// | ||
343 | template<typename MotionDerived> | ||
344 | SE3Tpl< | ||
345 | typename MotionDerived::Scalar, | ||
346 | PINOCCHIO_EIGEN_PLAIN_TYPE(typename MotionDerived::Vector3)::Options> | ||
347 | 13 | exp6(const MotionDense<MotionDerived> & nu) | |
348 | { | ||
349 | typedef typename MotionDerived::Scalar Scalar; | ||
350 | enum | ||
351 | { | ||
352 | Options = PINOCCHIO_EIGEN_PLAIN_TYPE(typename MotionDerived::Vector3)::Options | ||
353 | }; | ||
354 | |||
355 | typedef SE3Tpl<Scalar, Options> SE3; | ||
356 | |||
357 |
1/2✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
|
13 | SE3 res; |
358 |
1/2✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
|
13 | typename SE3::LinearType & trans = res.translation(); |
359 |
1/2✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
|
13 | typename SE3::AngularType & rot = res.rotation(); |
360 | |||
361 |
1/2✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
|
13 | const typename MotionDerived::ConstAngularType & w = nu.angular(); |
362 |
1/2✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
|
13 | const typename MotionDerived::ConstLinearType & v = nu.linear(); |
363 | const static Scalar eps = Eigen::NumTraits<Scalar>::epsilon(); | ||
364 | |||
365 | Scalar alpha_wxv, alpha_v, alpha_w, diagonal_term; | ||
366 |
1/2✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
|
13 | const Scalar t2 = w.squaredNorm() + eps * eps; |
367 | 13 | const Scalar t = math::sqrt(t2); | |
368 | Scalar ct, st; | ||
369 | 13 | SINCOS(t, &st, &ct); | |
370 | 13 | const Scalar inv_t2 = Scalar(1) / t2; | |
371 | |||
372 | 26 | alpha_wxv = internal::if_then_else( | |
373 |
1/2✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
|
13 | internal::LT, t, TaylorSeriesExpansion<Scalar>::template precision<3>(), |
374 | ✗ | static_cast<Scalar>(Scalar(1) / Scalar(2) - t2 / 24), | |
375 |
1/2✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
|
13 | static_cast<Scalar>((Scalar(1) - ct) * inv_t2)); |
376 | |||
377 | 26 | alpha_v = internal::if_then_else( | |
378 |
1/2✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
|
13 | internal::LT, t, TaylorSeriesExpansion<Scalar>::template precision<3>(), |
379 |
1/2✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
|
13 | static_cast<Scalar>(Scalar(1) - t2 / 6), static_cast<Scalar>((st) / t)); |
380 | |||
381 | 26 | alpha_w = internal::if_then_else( | |
382 |
1/2✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
|
13 | internal::LT, t, TaylorSeriesExpansion<Scalar>::template precision<3>(), |
383 | ✗ | static_cast<Scalar>((Scalar(1) / Scalar(6) - t2 / 120)), | |
384 |
1/2✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
|
13 | static_cast<Scalar>((Scalar(1) - alpha_v) * inv_t2)); |
385 | |||
386 | 26 | diagonal_term = internal::if_then_else( | |
387 |
1/2✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
|
13 | internal::LT, t, TaylorSeriesExpansion<Scalar>::template precision<3>(), |
388 |
1/2✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
|
13 | static_cast<Scalar>(Scalar(1) - t2 / 2), ct); |
389 | |||
390 | // Linear | ||
391 |
9/18✓ 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 taken 13 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 13 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 13 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 13 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 13 times.
✗ Branch 26 not taken.
|
13 | trans.noalias() = (alpha_v * v + (alpha_w * w.dot(v)) * w + alpha_wxv * w.cross(v)); |
392 | |||
393 | // Rotational | ||
394 |
5/10✓ 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 taken 13 times.
✗ Branch 14 not taken.
|
13 | rot.noalias() = alpha_wxv * w * w.transpose(); |
395 |
2/4✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 13 times.
✗ Branch 5 not taken.
|
13 | rot.coeffRef(0, 1) -= alpha_v * w[2]; |
396 |
2/4✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 13 times.
✗ Branch 5 not taken.
|
13 | rot.coeffRef(1, 0) += alpha_v * w[2]; |
397 |
2/4✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 13 times.
✗ Branch 5 not taken.
|
13 | rot.coeffRef(0, 2) += alpha_v * w[1]; |
398 |
2/4✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 13 times.
✗ Branch 5 not taken.
|
13 | rot.coeffRef(2, 0) -= alpha_v * w[1]; |
399 |
2/4✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 13 times.
✗ Branch 5 not taken.
|
13 | rot.coeffRef(1, 2) -= alpha_v * w[0]; |
400 |
2/4✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 13 times.
✗ Branch 5 not taken.
|
13 | rot.coeffRef(2, 1) += alpha_v * w[0]; |
401 |
3/6✓ 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.
|
13 | rot.diagonal().array() += diagonal_term; |
402 | |||
403 | 26 | return res; | |
404 | } | ||
405 | |||
406 | /// \brief Exp: se3 -> SE3. | ||
407 | /// | ||
408 | /// Return the integral of the input spatial velocity during time 1. | ||
409 | /// | ||
410 | /// \param[in] v The twist represented by a vector. | ||
411 | /// | ||
412 | /// \return The rigid transformation associated to the integration of the twist vector during | ||
413 | /// time 1. | ||
414 | /// | ||
415 | template<typename Vector6Like> | ||
416 | SE3Tpl<typename Vector6Like::Scalar, PINOCCHIO_EIGEN_PLAIN_TYPE(Vector6Like)::Options> | ||
417 | ✗ | exp6(const Eigen::MatrixBase<Vector6Like> & v) | |
418 | { | ||
419 | ✗ | PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE(Vector6Like, v, 6, 1); | |
420 | |||
421 | ✗ | MotionRef<const Vector6Like> nu(v.derived()); | |
422 | ✗ | return exp6(nu); | |
423 | } | ||
424 | |||
425 | /// \brief Log: SE3 -> se3. | ||
426 | /// | ||
427 | /// Pseudo-inverse of exp from \f$ SE3 \to { v,\omega \in \mathfrak{se}(3), ||\omega|| < 2\pi } | ||
428 | /// \f$. | ||
429 | /// | ||
430 | /// \param[in] M The rigid transformation. | ||
431 | /// | ||
432 | /// \return The twist associated to the rigid transformation during time 1. | ||
433 | /// | ||
434 | template<typename Scalar, int Options> | ||
435 | 1266 | MotionTpl<Scalar, Options> log6(const SE3Tpl<Scalar, Options> & M) | |
436 | { | ||
437 | typedef MotionTpl<Scalar, Options> Motion; | ||
438 | 1266 | Motion mout; | |
439 | 1266 | log6_impl<Scalar>::run(M, mout); | |
440 | 1266 | return mout; | |
441 | } | ||
442 | |||
443 | /// \brief Log: SE3 -> se3. | ||
444 | /// | ||
445 | /// Pseudo-inverse of exp from \f$ SE3 \to { v,\omega \in \mathfrak{se}(3), ||\omega|| < 2\pi } | ||
446 | /// \f$, using the quaternion representation of the rotation. | ||
447 | /// | ||
448 | /// \param[in] quat The rotation quaternion. | ||
449 | /// \param[in] vec The translation vector. | ||
450 | /// | ||
451 | /// \return The twist associated to the rigid transformation during time 1. | ||
452 | /// | ||
453 | template<typename Vector3Like, typename QuaternionLike> | ||
454 | 130 | MotionTpl<typename Vector3Like::Scalar, Vector3Like::Options> log6( | |
455 | const Eigen::QuaternionBase<QuaternionLike> & quat, const Eigen::MatrixBase<Vector3Like> & vec) | ||
456 | { | ||
457 | typedef typename Vector3Like::Scalar Scalar; | ||
458 | typedef MotionTpl<Scalar, Vector3Like::Options> Motion; | ||
459 | 130 | Motion mout; | |
460 | 130 | log6_impl<Scalar>::run(quat, vec, mout); | |
461 | 130 | return mout; | |
462 | } | ||
463 | |||
464 | /// \brief Log: SE3 -> se3. | ||
465 | /// | ||
466 | /// Pseudo-inverse of exp from \f$ SE3 \to { v,\omega \in \mathfrak{se}(3), ||\omega|| < 2\pi } | ||
467 | /// \f$. | ||
468 | /// | ||
469 | /// \param[in] M The rigid transformation represented as an homogenous matrix. | ||
470 | /// | ||
471 | /// \return The twist associated to the rigid transformation during time 1. | ||
472 | /// | ||
473 | template<typename Matrix4Like> | ||
474 | MotionTpl<typename Matrix4Like::Scalar, Eigen::internal::traits<Matrix4Like>::Options> | ||
475 | ✗ | log6(const Eigen::MatrixBase<Matrix4Like> & M) | |
476 | { | ||
477 | ✗ | PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix4Like, M, 4, 4); | |
478 | |||
479 | typedef typename Matrix4Like::Scalar Scalar; | ||
480 | enum | ||
481 | { | ||
482 | Options = Eigen::internal::traits<Matrix4Like>::Options | ||
483 | }; | ||
484 | typedef MotionTpl<Scalar, Options> Motion; | ||
485 | typedef SE3Tpl<Scalar, Options> SE3; | ||
486 | |||
487 | ✗ | SE3 m(M); | |
488 | ✗ | Motion mout; | |
489 | ✗ | log6_impl<Scalar>::run(m, mout); | |
490 | ✗ | return mout; | |
491 | } | ||
492 | |||
493 | /// \brief Derivative of exp6 | ||
494 | /// Computed as the inverse of Jlog6 | ||
495 | template<AssignmentOperatorType op, typename MotionDerived, typename Matrix6Like> | ||
496 | 52 | void Jexp6(const MotionDense<MotionDerived> & nu, const Eigen::MatrixBase<Matrix6Like> & Jexp) | |
497 | { | ||
498 |
2/4✓ Branch 1 taken 26 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 26 times.
✗ Branch 5 not taken.
|
52 | PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix6Like, Jexp, 6, 6); |
499 | |||
500 | typedef typename MotionDerived::Scalar Scalar; | ||
501 | typedef typename MotionDerived::Vector3 Vector3; | ||
502 | typedef Eigen::Matrix<Scalar, 3, 3, Vector3::Options> Matrix3; | ||
503 | 52 | Matrix6Like & Jout = PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like, Jexp); | |
504 | |||
505 |
1/2✓ Branch 1 taken 26 times.
✗ Branch 2 not taken.
|
52 | const typename MotionDerived::ConstLinearType & v = nu.linear(); |
506 |
1/2✓ Branch 1 taken 26 times.
✗ Branch 2 not taken.
|
52 | const typename MotionDerived::ConstAngularType & w = nu.angular(); |
507 |
1/2✓ Branch 1 taken 26 times.
✗ Branch 2 not taken.
|
52 | const Scalar t2 = w.squaredNorm(); |
508 | 52 | const Scalar t = math::sqrt(t2); | |
509 | |||
510 | 52 | const Scalar tinv = Scalar(1) / t, t2inv = tinv * tinv; | |
511 | Scalar st, ct; | ||
512 | 52 | SINCOS(t, &st, &ct); | |
513 | 52 | const Scalar inv_2_2ct = Scalar(1) / (Scalar(2) * (Scalar(1) - ct)); | |
514 | |||
515 | 104 | const Scalar beta = internal::if_then_else( | |
516 |
1/2✓ Branch 1 taken 26 times.
✗ Branch 2 not taken.
|
52 | internal::LT, t, TaylorSeriesExpansion<Scalar>::template precision<3>(), |
517 | ✗ | static_cast<Scalar>(Scalar(1) / Scalar(12) + t2 / Scalar(720)), | |
518 |
1/2✓ Branch 1 taken 26 times.
✗ Branch 2 not taken.
|
52 | static_cast<Scalar>(t2inv - st * tinv * inv_2_2ct)); |
519 | |||
520 | 104 | const Scalar beta_dot_over_theta = internal::if_then_else( | |
521 |
1/2✓ Branch 1 taken 26 times.
✗ Branch 2 not taken.
|
52 | internal::LT, t, TaylorSeriesExpansion<Scalar>::template precision<3>(), |
522 | ✗ | static_cast<Scalar>(Scalar(1) / Scalar(360)), | |
523 | ✗ | static_cast<Scalar>( | |
524 |
1/2✓ Branch 1 taken 26 times.
✗ Branch 2 not taken.
|
52 | -Scalar(2) * t2inv * t2inv + (Scalar(1) + st * tinv) * t2inv * inv_2_2ct)); |
525 | |||
526 | switch (op) | ||
527 | { | ||
528 | case SETTO: { | ||
529 |
2/4✓ Branch 1 taken 24 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 24 times.
✗ Branch 5 not taken.
|
48 | Jexp3<SETTO>(w, Jout.template bottomRightCorner<3, 3>()); |
530 |
3/6✓ Branch 1 taken 24 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 24 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 24 times.
✗ Branch 8 not taken.
|
48 | Jout.template topLeftCorner<3, 3>() = Jout.template bottomRightCorner<3, 3>(); |
531 |
4/8✓ Branch 1 taken 24 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 24 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 24 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 24 times.
✗ Branch 11 not taken.
|
48 | const Vector3 p = Jout.template topLeftCorner<3, 3>().transpose() * v; |
532 |
1/2✓ Branch 1 taken 24 times.
✗ Branch 2 not taken.
|
48 | const Scalar wTp(w.dot(p)); |
533 |
11/22✓ Branch 1 taken 24 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 24 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 24 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 24 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 24 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 24 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 24 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 24 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 24 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 24 times.
✗ Branch 29 not taken.
✓ Branch 31 taken 24 times.
✗ Branch 32 not taken.
|
144 | const Matrix3 J( |
534 |
2/4✓ Branch 1 taken 24 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 24 times.
✗ Branch 5 not taken.
|
48 | alphaSkew(.5, p) + (beta_dot_over_theta * wTp) * w * w.transpose() |
535 |
2/4✓ Branch 1 taken 24 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 24 times.
✗ Branch 5 not taken.
|
48 | - (t2 * beta_dot_over_theta + Scalar(2) * beta) * p * w.transpose() |
536 |
2/4✓ Branch 1 taken 24 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 24 times.
✗ Branch 5 not taken.
|
96 | + wTp * beta * Matrix3::Identity() + beta * w * p.transpose()); |
537 |
6/12✓ Branch 1 taken 24 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 24 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 24 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 24 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 24 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 24 times.
✗ Branch 17 not taken.
|
48 | Jout.template topRightCorner<3, 3>().noalias() = -Jout.template topLeftCorner<3, 3>() * J; |
538 |
2/4✓ Branch 1 taken 24 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 24 times.
✗ Branch 5 not taken.
|
48 | Jout.template bottomLeftCorner<3, 3>().setZero(); |
539 | 48 | break; | |
540 | } | ||
541 | case ADDTO: { | ||
542 | PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH | ||
543 | PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED | ||
544 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | Matrix3 Jtmp3; |
545 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | Jexp3<SETTO>(w, Jtmp3); |
546 | PINOCCHIO_COMPILER_DIAGNOSTIC_POP | ||
547 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | Jout.template bottomRightCorner<3, 3>() += Jtmp3; |
548 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | Jout.template topLeftCorner<3, 3>() += Jtmp3; |
549 |
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.
|
2 | const Vector3 p = Jtmp3.transpose() * v; |
550 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | const Scalar wTp(w.dot(p)); |
551 |
11/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 taken 1 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 1 times.
✗ Branch 29 not taken.
✓ Branch 31 taken 1 times.
✗ Branch 32 not taken.
|
6 | const Matrix3 J( |
552 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | alphaSkew(.5, p) + (beta_dot_over_theta * wTp) * w * w.transpose() |
553 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | - (t2 * beta_dot_over_theta + Scalar(2) * beta) * p * w.transpose() |
554 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
4 | + wTp * beta * Matrix3::Identity() + beta * w * p.transpose()); |
555 |
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.
|
2 | Jout.template topRightCorner<3, 3>().noalias() += -Jtmp3 * J; |
556 | 2 | break; | |
557 | } | ||
558 | case RMTO: { | ||
559 | PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH | ||
560 | PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED | ||
561 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | Matrix3 Jtmp3; |
562 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | Jexp3<SETTO>(w, Jtmp3); |
563 | PINOCCHIO_COMPILER_DIAGNOSTIC_POP | ||
564 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | Jout.template bottomRightCorner<3, 3>() -= Jtmp3; |
565 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | Jout.template topLeftCorner<3, 3>() -= Jtmp3; |
566 |
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.
|
2 | const Vector3 p = Jtmp3.transpose() * v; |
567 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | const Scalar wTp(w.dot(p)); |
568 |
11/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 taken 1 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 1 times.
✗ Branch 29 not taken.
✓ Branch 31 taken 1 times.
✗ Branch 32 not taken.
|
6 | const Matrix3 J( |
569 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | alphaSkew(.5, p) + (beta_dot_over_theta * wTp) * w * w.transpose() |
570 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | - (t2 * beta_dot_over_theta + Scalar(2) * beta) * p * w.transpose() |
571 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
4 | + wTp * beta * Matrix3::Identity() + beta * w * p.transpose()); |
572 |
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.
|
2 | Jout.template topRightCorner<3, 3>().noalias() -= -Jtmp3 * J; |
573 | 2 | break; | |
574 | } | ||
575 | default: | ||
576 | assert(false && "Wrong Op requesed value"); | ||
577 | break; | ||
578 | } | ||
579 | 52 | } | |
580 | |||
581 | /// \brief Derivative of exp6 | ||
582 | /// Computed as the inverse of Jlog6 | ||
583 | template<typename MotionDerived, typename Matrix6Like> | ||
584 | 13 | void Jexp6(const MotionDense<MotionDerived> & nu, const Eigen::MatrixBase<Matrix6Like> & Jexp) | |
585 | { | ||
586 | 13 | Jexp6<SETTO>(nu, Jexp); | |
587 | 13 | } | |
588 | |||
589 | /// \brief Derivative of exp6 | ||
590 | /// Computed as the inverse of Jlog6 | ||
591 | template<typename MotionDerived> | ||
592 | Eigen::Matrix<typename MotionDerived::Scalar, 6, 6, MotionDerived::Options> | ||
593 | ✗ | Jexp6(const MotionDense<MotionDerived> & nu) | |
594 | { | ||
595 | typedef typename MotionDerived::Scalar Scalar; | ||
596 | enum | ||
597 | { | ||
598 | Options = MotionDerived::Options | ||
599 | }; | ||
600 | typedef Eigen::Matrix<Scalar, 6, 6, Options> ReturnType; | ||
601 | |||
602 | ✗ | ReturnType res; | |
603 | ✗ | Jexp6(nu, res); | |
604 | ✗ | return res; | |
605 | } | ||
606 | |||
607 | /** \brief Derivative of log6 | ||
608 | * | ||
609 | * This function is the right derivative of @ref log6, that is, for \f$M \in | ||
610 | * SE(3)\f$ and \f$\xi in \mathfrak{se}(3)\f$, it provides the linear | ||
611 | * approximation: | ||
612 | * | ||
613 | * \f[ | ||
614 | * \log_6(M \oplus \xi) = \log_6(M \exp_6(\xi)) \approx \log_6(M) + \text{Jlog6}(M) \xi | ||
615 | * \f] | ||
616 | * | ||
617 | * Equivalently, \f$\text{Jlog6}\f$ is the right Jacobian of \f$\log_6\f$: | ||
618 | * | ||
619 | * \f[ | ||
620 | * \text{Jlog6}(M) = \frac{\partial \log_6(M)}{\partial M} | ||
621 | * \f] | ||
622 | * | ||
623 | * Note that this is the right Jacobian: \f$\text{Jlog6}(M) : T_{M} SE(3) \to T_{\log_6(M)} | ||
624 | * \mathfrak{se}(3)\f$. (By convention, calculations in Pinocchio always perform right | ||
625 | * differentiation, i.e., Jacobians are in local coordinates (also known as body coordinates), | ||
626 | * unless otherwise specified.) | ||
627 | * | ||
628 | * Internally, it is calculated using the following formulas: | ||
629 | * | ||
630 | * \f[ | ||
631 | * \text{Jlog6}(M) = | ||
632 | * \left(\begin{array}{cc} | ||
633 | * \text{Jlog3}(R) & J * \text{Jlog3}(R) \\ | ||
634 | * 0 & \text{Jlog3}(R) \\ | ||
635 | * \end{array}\right) | ||
636 | * \f] | ||
637 | * where | ||
638 | * \f[ M = | ||
639 | * \left(\begin{array}{cc} | ||
640 | * \exp(\mathbf{r}) & \mathbf{p} \\ | ||
641 | * 0 & 1 \\ | ||
642 | * \end{array}\right) | ||
643 | * \f] | ||
644 | * \f[ | ||
645 | * \begin{eqnarray} | ||
646 | * J &=& | ||
647 | * \left.\frac{1}{2}[\mathbf{p}]_{\times} + \beta'(||r||) | ||
648 | * \frac{\mathbf{r}^T\mathbf{p}}{||r||}\mathbf{r}\mathbf{r}^T | ||
649 | * - (||r||\beta'(||r||) + 2 \beta(||r||)) \mathbf{p}\mathbf{r}^T\right.\\ | ||
650 | * &&\left. + \mathbf{r}^T\mathbf{p}\beta(||r||)I_3 + \beta(||r||)\mathbf{r}\mathbf{p}^T\right. | ||
651 | * \end{eqnarray} | ||
652 | * \f] | ||
653 | * and | ||
654 | * \f[ \beta(x)=\left(\frac{1}{x^2} - \frac{\sin x}{2x(1-\cos x)}\right) \f] | ||
655 | * | ||
656 | * | ||
657 | * \cheatsheet For \f$(A,B) \in SE(3)^2\f$, let \f$M_1(A, B) = A B\f$ and | ||
658 | * \f$m_1 = \log_6(M_1) \f$. Then, we have the following partial (right) | ||
659 | * Jacobians: \n | ||
660 | * - \f$ \frac{\partial m_1}{\partial A} = Jlog_6(M_1) Ad_B^{-1} \f$, | ||
661 | * - \f$ \frac{\partial m_1}{\partial B} = Jlog_6(M_1) \f$. | ||
662 | * | ||
663 | * \cheatsheet Let \f$A \in SE(3)\f$, \f$M_2(A) = A^{-1}\f$ and \f$m_2 = | ||
664 | * \log_6(M_2)\f$. Then, we have the following partial (right) Jacobian: \n | ||
665 | * - \f$ \frac{\partial m_2}{\partial A} = - Jlog_6(M_2) Ad_A \f$. | ||
666 | */ | ||
667 | template<typename Scalar, int Options, typename Matrix6Like> | ||
668 | 124 | void Jlog6(const SE3Tpl<Scalar, Options> & M, const Eigen::MatrixBase<Matrix6Like> & Jlog) | |
669 | { | ||
670 | 124 | Jlog6_impl<Scalar>::run(M, PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like, Jlog)); | |
671 | 124 | } | |
672 | |||
673 | /// | ||
674 | /// \copydoc Jlog6(const SE3Tpl<Scalar, Options> &, const Eigen::MatrixBase<Matrix6Like> &) | ||
675 | /// | ||
676 | /// \param[in] M The rigid transformation. | ||
677 | /// | ||
678 | template<typename Scalar, int Options> | ||
679 | 1 | Eigen::Matrix<Scalar, 6, 6, Options> Jlog6(const SE3Tpl<Scalar, Options> & M) | |
680 | { | ||
681 | typedef Eigen::Matrix<Scalar, 6, 6, Options> ReturnType; | ||
682 | |||
683 | 1 | ReturnType res; | |
684 | 1 | Jlog6(M, res); | |
685 | 1 | return res; | |
686 | } | ||
687 | |||
688 | template<typename Scalar, int Options> | ||
689 | template<typename OtherScalar> | ||
690 | ✗ | SE3Tpl<Scalar, Options> SE3Tpl<Scalar, Options>::Interpolate( | |
691 | const SE3Tpl & A, const SE3Tpl & B, const OtherScalar & alpha) | ||
692 | { | ||
693 | typedef SE3Tpl<Scalar, Options> ReturnType; | ||
694 | typedef MotionTpl<Scalar, Options> Motion; | ||
695 | |||
696 | ✗ | Motion dv = log6(A.actInv(B)); | |
697 | ✗ | ReturnType res = A * exp6(alpha * dv); | |
698 | ✗ | return res; | |
699 | } | ||
700 | |||
701 | } // namespace pinocchio | ||
702 | |||
703 | #include "pinocchio/spatial/explog-quaternion.hpp" | ||
704 | #include "pinocchio/spatial/log.hxx" | ||
705 | |||
706 | #endif // #ifndef __pinocchio_spatial_explog_hpp__ | ||
707 |