GCC Code Coverage Report

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