GCC Code Coverage Report
Directory: ./ Exec Total Coverage
File: include/pinocchio/spatial/explog.hpp Lines: 191 201 95.0 %
Date: 2024-04-26 13:14:21 Branches: 313 624 50.2 %

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 time 1.
31
  ///
32
  template<typename Vector3Like>
33
  typename Eigen::Matrix<typename Vector3Like::Scalar,3,3,PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like)::Options>
34
2135
  exp3(const Eigen::MatrixBase<Vector3Like> & v)
35
  {
36


2135
    PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE (Vector3Like, v, 3, 1);
37
38
    typedef typename Vector3Like::Scalar Scalar;
39
    typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like) Vector3LikePlain;
40
    typedef Eigen::Matrix<Scalar,3,3,Vector3LikePlain::Options> Matrix3;
41
42
2135
    const Scalar t2 = v.squaredNorm();
43
44
2135
    const Scalar t = math::sqrt(t2);
45
2135
    Scalar ct,st; SINCOS(t,&st,&ct);
46
47
2135
    const Scalar alpha_vxvx = internal::if_then_else(internal::GT, t, TaylorSeriesExpansion<Scalar>::template precision<3>(),
48
                                                     (1 - ct)/t2,
49
2135
                                                     Scalar(1)/Scalar(2) - t2/24);
50
2135
    const Scalar alpha_vx = internal::if_then_else(internal::GT, t, TaylorSeriesExpansion<Scalar>::template precision<3>(),
51
                                                   (st)/t,
52
2135
                                                   Scalar(1) - t2/6);
53


2135
    Matrix3 res(alpha_vxvx * v * v.transpose());
54


2135
    res.coeffRef(0,1) -= alpha_vx * v[2]; res.coeffRef(1,0) += alpha_vx * v[2];
55


2135
    res.coeffRef(0,2) += alpha_vx * v[1]; res.coeffRef(2,0) -= alpha_vx * v[1];
56


2135
    res.coeffRef(1,2) -= alpha_vx * v[0]; res.coeffRef(2,1) += alpha_vx * v[0];
57
58
2135
    ct = internal::if_then_else(internal::GT, t, TaylorSeriesExpansion<Scalar>::template precision<3>(),
59
                                ct,
60
2135
                                Scalar(1) - t2/2);
61

2135
    res.diagonal().array() += ct;
62
63
4270
    return res;
64
  }
65
66
  /// \brief Same as \ref log3
67
  ///
68
  /// \param[in] R the rotation matrix.
69
  /// \param[out] theta the angle value.
70
  ///
71
  /// \return The angular velocity vector associated to the rotation matrix.
72
  ///
73
  template<typename Matrix3Like>
74
  Eigen::Matrix<typename Matrix3Like::Scalar,3,1,PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix3Like)::Options>
75
21810
  log3(const Eigen::MatrixBase<Matrix3Like> & R,
76
       typename Matrix3Like::Scalar & theta)
77
  {
78
    typedef typename Matrix3Like::Scalar Scalar;
79
    typedef Eigen::Matrix<Scalar,3,1,
80
                          PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix3Like)::Options> Vector3;
81
21810
    Vector3 res;
82
21810
    log3_impl<Scalar>::run(R, theta, res);
83
21810
    return res;
84
  }
85
86
  /// \brief Log: SO(3)-> so(3).
87
  ///
88
  /// Pseudo-inverse of log from \f$ SO3 -> { v \in so3, ||v|| \le pi } \f$.
89
  ///
90
  /// \param[in] R The rotation matrix.
91
  ///
92
  /// \return The angular velocity vector associated to the rotation matrix.
93
  ///
94
  template<typename Matrix3Like>
95
  Eigen::Matrix<typename Matrix3Like::Scalar,3,1,PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix3Like)::Options>
96
36
  log3(const Eigen::MatrixBase<Matrix3Like> & R)
97
  {
98
    typename Matrix3Like::Scalar theta;
99
72
    return log3(R.derived(),theta);
100
  }
101
102
  ///
103
  /// \brief Derivative of \f$ \exp{r} \f$
104
  /// \f[
105
  ///     \frac{\sin{||r||}}{||r||}                       I_3
106
  ///   - \frac{1-\cos{||r||}}{||r||^2}                   \left[ r \right]_x
107
  ///   + \frac{1}{||n||^2} (1-\frac{\sin{||r||}}{||r||}) r r^T
108
  /// \f]
109
  ///
110
  template<AssignmentOperatorType op, typename Vector3Like, typename Matrix3Like>
111
434
  void Jexp3(const Eigen::MatrixBase<Vector3Like> & r,
112
             const Eigen::MatrixBase<Matrix3Like> & Jexp)
113
  {
114


434
    PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE (Vector3Like, r   , 3, 1);
115


434
    PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE (Matrix3Like, Jexp, 3, 3);
116
117
434
    Matrix3Like & Jout = PINOCCHIO_EIGEN_CONST_CAST(Matrix3Like,Jexp);
118
    typedef typename Matrix3Like::Scalar Scalar;
119
120
434
    const Scalar n2 = r.squaredNorm();
121
434
    const Scalar n = math::sqrt(n2);
122
434
    const Scalar n_inv = Scalar(1)/n;
123
434
    const Scalar n2_inv = n_inv * n_inv;
124
434
    Scalar cn,sn; SINCOS(n,&sn,&cn);
125
126
434
    const Scalar a = internal::if_then_else(internal::LT, n, TaylorSeriesExpansion<Scalar>::template precision<3>(),
127
                                            Scalar(1) - n2/Scalar(6),
128
434
                                            sn*n_inv);
129
434
    const Scalar b = internal::if_then_else(internal::LT, n, TaylorSeriesExpansion<Scalar>::template precision<3>(),
130
                                            - Scalar(1)/Scalar(2) - n2/Scalar(24),
131
434
                                            - (1-cn)*n2_inv);
132
434
    const Scalar c = internal::if_then_else(internal::LT, n, TaylorSeriesExpansion<Scalar>::template precision<3>(),
133
                                            Scalar(1)/Scalar(6) - n2/Scalar(120),
134
434
                                            n2_inv * (1 - a));
135
136
    switch(op)
137
      {
138
      case SETTO:
139

430
        Jout.diagonal().setConstant(a);
140


430
        Jout(0,1) = -b*r[2]; Jout(1,0) = -Jout(0,1);
141


430
        Jout(0,2) =  b*r[1]; Jout(2,0) = -Jout(0,2);
142


430
        Jout(1,2) = -b*r[0]; Jout(2,1) = -Jout(1,2);
143


430
        Jout.noalias() += c * r * r.transpose();
144
430
        break;
145
      case ADDTO:
146

2
        Jout.diagonal().array() += a;
147


2
        Jout(0,1) += -b*r[2]; Jout(1,0) += b*r[2];
148


2
        Jout(0,2) +=  b*r[1]; Jout(2,0) += -b*r[1];
149


2
        Jout(1,2) += -b*r[0]; Jout(2,1) += b*r[0];
150


2
        Jout.noalias() += c * r * r.transpose();
151
2
        break;
152
      case RMTO:
153

2
        Jout.diagonal().array() -= a;
154


2
        Jout(0,1) -= -b*r[2]; Jout(1,0) -= b*r[2];
155


2
        Jout(0,2) -=  b*r[1]; Jout(2,0) -= -b*r[1];
156


2
        Jout(1,2) -= -b*r[0]; Jout(2,1) -= b*r[0];
157


2
        Jout.noalias() -= c * r * r.transpose();
158
2
        break;
159
      default:
160
        assert(false && "Wrong Op requesed value");
161
        break;
162
      }
163
434
  }
164
165
  ///
166
  /// \brief Derivative of \f$ \exp{r} \f$
167
  /// \f[
168
  ///     \frac{\sin{||r||}}{||r||}                       I_3
169
  ///   - \frac{1-\cos{||r||}}{||r||^2}                   \left[ r \right]_x
170
  ///   + \frac{1}{||n||^2} (1-\frac{\sin{||r||}}{||r||}) r r^T
171
  /// \f]
172
  ///
173
  template<typename Vector3Like, typename Matrix3Like>
174
8
  void Jexp3(const Eigen::MatrixBase<Vector3Like> & r,
175
             const Eigen::MatrixBase<Matrix3Like> & Jexp)
176
  {
177
8
    Jexp3<SETTO>(r, Jexp);
178
8
  }
179
180
  /** \brief Derivative of log3
181
   *
182
   * This function is the right derivative of @ref log3, that is, for \f$R \in
183
   * SO(3)\f$ and \f$\omega t in \mathfrak{so}(3)\f$, it provides the linear
184
   * approximation:
185
   *
186
   * \f[
187
   * \log_3(R \oplus \omega t) = \log_3(R \exp_3(\omega t)) \approx \log_3(R) + \text{Jlog3}(R) \omega t
188
   * \f]
189
   *
190
   *  \param[in] theta the angle value.
191
   *  \param[in] log the output of log3.
192
   *  \param[out] Jlog the jacobian
193
   *
194
   * Equivalently, \f$\text{Jlog3}\f$ is the right Jacobian of \f$\log_3\f$:
195
   *
196
   * \f[
197
   * \text{Jlog3}(R) = \frac{\partial \log_3(R)}{\partial R}
198
   * \f]
199
   *
200
   * Note that this is the right Jacobian: \f$\text{Jlog3}(R) : T_{R} SO(3) \to T_{\log_6(R)} \mathfrak{so}(3)\f$.
201
   * (By convention, calculations in Pinocchio always perform right differentiation,
202
   * i.e., Jacobians are in local coordinates (also known as body coordinates), unless otherwise specified.)
203
   *
204
   * If we denote by \f$\theta = \log_3(R)\f$ and \f$\log = \log_3(R,
205
   * \theta)\f$, then \f$\text{Jlog} = \text{Jlog}_3(R)\f$ can be calculated as:
206
   *
207
   *  \f[
208
   *  \begin{align*}
209
   *  \text{Jlog} & = \frac{\theta \sin(\theta)}{2 (1 - \cos(\theta))} I_3
210
   *             + \frac{1}{2} \widehat{\log}
211
   *             + \left(\frac{1}{\theta^2} - \frac{\sin(\theta)}{2\theta(1-\cos(\theta))}\right) \log \log^T \\
212
   *             & = I_3
213
   *             + \frac{1}{2} \widehat{\log}
214
   *             + \left(\frac{1}{\theta^2} - \frac{1 + \cos \theta}{2 \theta \sin \theta}\right) \widehat{\log}^2
215
   *  \end{align*}
216
   *  \f]
217
   *
218
   *  where \f$\widehat{v}\f$ denotes the skew-symmetric matrix obtained from the 3D vector \f$v\f$.
219
   *
220
   *  \note The inputs must be such that \f$ \theta = \Vert \log \Vert \f$.
221
   */
222
  template<typename Scalar, typename Vector3Like, typename Matrix3Like>
223
992
  void Jlog3(const Scalar & theta,
224
             const Eigen::MatrixBase<Vector3Like> & log,
225
             const Eigen::MatrixBase<Matrix3Like> & Jlog)
226
  {
227
992
    Jlog3_impl<Scalar>::run(theta, log,
228
992
                            PINOCCHIO_EIGEN_CONST_CAST(Matrix3Like,Jlog));
229
992
  }
230
231
  /** \brief Derivative of log3
232
   *
233
   *  \param[in] R the rotation matrix.
234
   *  \param[out] Jlog the jacobian
235
   *
236
   *  Equivalent to
237
   *  \code
238
   *  double theta;
239
   *  Vector3 log = pinocchio::log3 (R, theta);
240
   *  pinocchio::Jlog3 (theta, log, Jlog);
241
   *  \endcode
242
   */
243
  template<typename Matrix3Like1, typename Matrix3Like2>
244
11
  void Jlog3(const Eigen::MatrixBase<Matrix3Like1> & R,
245
             const Eigen::MatrixBase<Matrix3Like2> & Jlog)
246
  {
247
    typedef typename Matrix3Like1::Scalar Scalar;
248
    typedef Eigen::Matrix<Scalar,3,1,PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix3Like1)::Options> Vector3;
249
250
    Scalar t;
251
11
    Vector3 w(log3(R,t));
252
11
    Jlog3(t,w,PINOCCHIO_EIGEN_CONST_CAST(Matrix3Like2,Jlog));
253
11
  }
254
255
  template<typename Scalar, typename Vector3Like1, typename Vector3Like2, typename Matrix3Like>
256
3
  void Hlog3(const Scalar & theta,
257
             const Eigen::MatrixBase<Vector3Like1> & log,
258
             const Eigen::MatrixBase<Vector3Like2> & v,
259
             const Eigen::MatrixBase<Matrix3Like> & vt_Hlog)
260
  {
261
    typedef Eigen::Matrix<Scalar,3,1,PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix3Like)::Options> Vector3;
262
3
    Matrix3Like & vt_Hlog_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix3Like,vt_Hlog);
263
264
    // theta = (log^T * log)^.5
265
    // dt/dl = .5 * 2 * log^T * (log^T * log)^-.5
266
    //       = log^T / theta
267
    // dt_dl = log / theta
268
3
    Scalar ctheta,stheta; SINCOS(theta,&stheta,&ctheta);
269
270
3
    Scalar denom = .5 / (1-ctheta),
271
3
           a = theta * stheta * denom,
272
3
           da_dt = (stheta - theta) * denom, // da / dtheta
273
3
           b = ( 1 - a ) / (theta*theta),
274
           //db_dt = - (2 * (1 - a) / theta + da_dt ) / theta**2; // db / dtheta
275
3
           db_dt = - (2 / theta - (theta + stheta) * denom) / (theta*theta); // db / dtheta
276
277
    // Compute dl_dv_v = Jlog * v
278
    // Jlog = a I3 + .5 [ log ] + b * log * log^T
279
    // if v == log, then Jlog * v == v
280





3
    Vector3 dl_dv_v (a*v + .5*log.cross(v) + b*log*log.transpose()*v);
281
282
3
    Scalar dt_dv_v = log.dot(dl_dv_v) / theta;
283
284
    // Derivative of b * log * log^T
285


3
    vt_Hlog_.noalias() = db_dt * dt_dv_v * log * log.transpose();
286


3
    vt_Hlog_.noalias() += b * dl_dv_v * log.transpose();
287


3
    vt_Hlog_.noalias() += b * log * dl_dv_v.transpose();
288
    // Derivative of .5 * [ log ]
289

3
    addSkew(.5 * dl_dv_v, vt_Hlog_);
290
    // Derivative of a * I3
291

3
    vt_Hlog_.diagonal().array() += da_dt * dt_dv_v;
292
3
  }
293
294
  /** \brief Second order derivative of log3
295
   *
296
   *  This computes \f$ v^T H_{log} \f$.
297
   *
298
   *  \param[in] R the rotation matrix.
299
   *  \param[in] v the 3D vector.
300
   *  \param[out] vt_Hlog the product of the Hessian with the input vector
301
   */
302
  template<typename Matrix3Like1, typename Vector3Like, typename Matrix3Like2>
303
3
  void Hlog3(const Eigen::MatrixBase<Matrix3Like1> & R,
304
             const Eigen::MatrixBase<Vector3Like> & v,
305
             const Eigen::MatrixBase<Matrix3Like2> & vt_Hlog)
306
  {
307
    typedef typename Matrix3Like1::Scalar Scalar;
308
    typedef Eigen::Matrix<Scalar,3,1,PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix3Like1)::Options> Vector3;
309
310
    Scalar t;
311
3
    Vector3 w(log3(R,t));
312
3
    Hlog3(t,w,v,PINOCCHIO_EIGEN_CONST_CAST(Matrix3Like2,vt_Hlog));
313
3
  }
314
315
  ///
316
  /// \brief Exp: se3 -> SE3.
317
  ///
318
  /// Return the integral of the input twist during time 1.
319
  ///
320
  /// \param[in] nu The input twist.
321
  ///
322
  /// \return The rigid transformation associated to the integration of the twist during time 1.
323
  ///
324
  template<typename MotionDerived>
325
  SE3Tpl<typename MotionDerived::Scalar,PINOCCHIO_EIGEN_PLAIN_TYPE(typename MotionDerived::Vector3)::Options>
326
82653
  exp6(const MotionDense<MotionDerived> & nu)
327
  {
328
    typedef typename MotionDerived::Scalar Scalar;
329
    enum { Options = PINOCCHIO_EIGEN_PLAIN_TYPE(typename MotionDerived::Vector3)::Options };
330
331
    typedef SE3Tpl<Scalar,Options> SE3;
332
333
82653
    SE3 res;
334
82653
    typename SE3::LinearType & trans = res.translation();
335
82653
    typename SE3::AngularType & rot = res.rotation();
336
337
82653
    const typename MotionDerived::ConstAngularType & w = nu.angular();
338
82653
    const typename MotionDerived::ConstLinearType & v = nu.linear();
339
340
    Scalar alpha_wxv, alpha_v, alpha_w, diagonal_term;
341
82653
    const Scalar t2 = w.squaredNorm();
342
82653
    const Scalar t = math::sqrt(t2);
343
82653
    Scalar ct,st; SINCOS(t,&st,&ct);
344
82653
    const Scalar inv_t2 = Scalar(1)/t2;
345
346
82653
    alpha_wxv = internal::if_then_else(internal::LT, t, TaylorSeriesExpansion<Scalar>::template precision<3>(),
347
                                       Scalar(1)/Scalar(2) - t2/24,
348
82653
                                       (Scalar(1) - ct)*inv_t2);
349
350
82653
    alpha_v = internal::if_then_else(internal::LT, t, TaylorSeriesExpansion<Scalar>::template precision<3>(),
351
                                     Scalar(1) - t2/6,
352
82653
                                     (st)/t);
353
354
82653
    alpha_w = internal::if_then_else(internal::LT, t, TaylorSeriesExpansion<Scalar>::template precision<3>(),
355
                                     (Scalar(1)/Scalar(6) - t2/120),
356
82653
                                     (Scalar(1) - alpha_v)*inv_t2);
357
358
82653
    diagonal_term = internal::if_then_else(internal::LT, t, TaylorSeriesExpansion<Scalar>::template precision<3>(),
359
82653
                                           Scalar(1) - t2/2,
360
                                           ct);
361
362
    // Linear
363




82653
    trans.noalias() = (alpha_v*v + (alpha_w*w.dot(v))*w + alpha_wxv*w.cross(v));
364
365
    // Rotational
366


82653
    rot.noalias() = alpha_wxv * w * w.transpose();
367


82653
    rot.coeffRef(0,1) -= alpha_v * w[2]; rot.coeffRef(1,0) += alpha_v * w[2];
368


82653
    rot.coeffRef(0,2) += alpha_v * w[1]; rot.coeffRef(2,0) -= alpha_v * w[1];
369


82653
    rot.coeffRef(1,2) -= alpha_v * w[0]; rot.coeffRef(2,1) += alpha_v * w[0];
370

82653
    rot.diagonal().array() += diagonal_term;
371
372
165306
    return res;
373
  }
374
375
  /// \brief Exp: se3 -> SE3.
376
  ///
377
  /// Return the integral of the input spatial velocity during time 1.
378
  ///
379
  /// \param[in] v The twist represented by a vector.
380
  ///
381
  /// \return The rigid transformation associated to the integration of the twist vector during time 1.
382
  ///
383
  template<typename Vector6Like>
384
  SE3Tpl<typename Vector6Like::Scalar,PINOCCHIO_EIGEN_PLAIN_TYPE(Vector6Like)::Options>
385
1
  exp6(const Eigen::MatrixBase<Vector6Like> & v)
386
  {
387


1
    PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE (Vector6Like, v, 6, 1);
388
389
1
    MotionRef<const Vector6Like> nu(v.derived());
390
2
    return exp6(nu);
391
  }
392
393
  /// \brief Log: SE3 -> se3.
394
  ///
395
  /// Pseudo-inverse of exp from \f$ SE3 \to { v,\omega \in \mathfrak{se}(3), ||\omega|| < 2\pi } \f$.
396
  ///
397
  /// \param[in] M The rigid transformation.
398
  ///
399
  /// \return The twist associated to the rigid transformation during time 1.
400
  ///
401
  template<typename Scalar, int Options>
402
  MotionTpl<Scalar,Options>
403
19396
  log6(const SE3Tpl<Scalar,Options> & M)
404
  {
405
    typedef MotionTpl<Scalar,Options> Motion;
406
19396
    Motion mout;
407
19396
    log6_impl<Scalar>::run(M, mout);
408
19396
    return mout;
409
  }
410
411
  /// \brief Log: SE3 -> se3.
412
  ///
413
  /// Pseudo-inverse of exp from \f$ SE3 \to { v,\omega \in \mathfrak{se}(3), ||\omega|| < 2\pi } \f$.
414
  ///
415
  /// \param[in] M The rigid transformation represented as an homogenous matrix.
416
  ///
417
  /// \return The twist associated to the rigid transformation during time 1.
418
  ///
419
  template<typename Matrix4Like>
420
  MotionTpl<typename Matrix4Like::Scalar,Eigen::internal::traits<Matrix4Like>::Options>
421
1
  log6(const Eigen::MatrixBase<Matrix4Like> & M)
422
  {
423


1
    PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix4Like, M, 4, 4);
424
425
    typedef typename Matrix4Like::Scalar Scalar;
426
    enum {Options = Eigen::internal::traits<Matrix4Like>::Options};
427
    typedef MotionTpl<Scalar,Options> Motion;
428
    typedef SE3Tpl<Scalar,Options> SE3;
429
430
1
    SE3 m(M);
431
1
    Motion mout;
432
1
    log6_impl<Scalar>::run(m, mout);
433
2
    return mout;
434
  }
435
436
  /// \brief Derivative of exp6
437
  /// Computed as the inverse of Jlog6
438
  template<AssignmentOperatorType op, typename MotionDerived, typename Matrix6Like>
439
232
  void Jexp6(const MotionDense<MotionDerived>     & nu,
440
             const Eigen::MatrixBase<Matrix6Like> & Jexp)
441
  {
442


232
    PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE (Matrix6Like, Jexp, 6, 6);
443
444
    typedef typename MotionDerived::Scalar Scalar;
445
    typedef typename MotionDerived::Vector3 Vector3;
446
    typedef Eigen::Matrix<Scalar, 3, 3, Vector3::Options> Matrix3;
447
232
    Matrix6Like & Jout = PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like,Jexp);
448
449
232
    const typename MotionDerived::ConstLinearType  & v = nu.linear();
450
232
    const typename MotionDerived::ConstAngularType & w = nu.angular();
451
232
    const Scalar t2 = w.squaredNorm();
452
232
    const Scalar t = math::sqrt(t2);
453
454
232
    const Scalar tinv = Scalar(1)/t,
455
232
                 t2inv = tinv*tinv;
456
232
    Scalar st,ct; SINCOS (t, &st, &ct);
457
232
    const Scalar inv_2_2ct = Scalar(1)/(Scalar(2)*(Scalar(1)-ct));
458
459
460
232
    const Scalar beta = internal::if_then_else(internal::LT, t, TaylorSeriesExpansion<Scalar>::template precision<3>(),
461
                                               Scalar(1)/Scalar(12) + t2/Scalar(720),
462
232
                                               t2inv - st*tinv*inv_2_2ct);
463
464
232
    const Scalar beta_dot_over_theta = internal::if_then_else(internal::LT, t, TaylorSeriesExpansion<Scalar>::template precision<3>(),
465
                                                              Scalar(1)/Scalar(360),
466
232
                                                              -Scalar(2)*t2inv*t2inv + (Scalar(1) + st*tinv) * t2inv * inv_2_2ct);
467
468
    switch(op)
469
      {
470
      case SETTO:
471
      {
472

228
        Jexp3<SETTO>(w, Jout.template bottomRightCorner<3,3>());
473

228
        Jout.template topLeftCorner<3,3>() = Jout.template bottomRightCorner<3,3>();
474


228
        const Vector3 p = Jout.template topLeftCorner<3,3>().transpose() * v;
475
228
        const Scalar wTp (w.dot (p));
476






912
        const Matrix3 J (alphaSkew(.5, p) +
477
228
                         (beta_dot_over_theta*wTp)                *w*w.transpose()
478
228
                         - (t2*beta_dot_over_theta+Scalar(2)*beta)*p*w.transpose()
479
228
                         + wTp * beta                             * Matrix3::Identity()
480
228
                         + beta                                   *w*p.transpose());
481



228
        Jout.template topRightCorner<3,3>().noalias() =
482
          - Jout.template topLeftCorner<3,3>() * J;
483

228
        Jout.template bottomLeftCorner<3,3>().setZero();
484
228
        break;
485
      }
486
      case ADDTO:
487
      {
488
PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
489
PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED
490
2
        Matrix3 Jtmp3;
491
2
        Jexp3<SETTO>(w, Jtmp3);
492
PINOCCHIO_COMPILER_DIAGNOSTIC_POP
493

2
        Jout.template bottomRightCorner<3,3>() += Jtmp3;
494

2
        Jout.template topLeftCorner<3,3>() += Jtmp3;
495

2
        const Vector3 p = Jtmp3.transpose() * v;
496
2
        const Scalar wTp (w.dot (p));
497






8
        const Matrix3 J (alphaSkew(.5, p) +
498
2
                         (beta_dot_over_theta*wTp)                *w*w.transpose()
499
2
                         - (t2*beta_dot_over_theta+Scalar(2)*beta)*p*w.transpose()
500
2
                         + wTp * beta                             * Matrix3::Identity()
501
2
                         + beta                                   *w*p.transpose());
502


2
        Jout.template topRightCorner<3,3>().noalias() +=
503
          - Jtmp3 * J;
504
2
        break;
505
      }
506
      case RMTO:
507
      {
508
PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
509
PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED
510
2
        Matrix3 Jtmp3;
511
2
        Jexp3<SETTO>(w, Jtmp3);
512
PINOCCHIO_COMPILER_DIAGNOSTIC_POP
513

2
        Jout.template bottomRightCorner<3,3>() -= Jtmp3;
514

2
        Jout.template topLeftCorner<3,3>() -= Jtmp3;
515

2
        const Vector3 p = Jtmp3.transpose() * v;
516
2
        const Scalar wTp (w.dot (p));
517






8
        const Matrix3 J (alphaSkew(.5, p) +
518
2
                         (beta_dot_over_theta*wTp)                *w*w.transpose()
519
2
                         - (t2*beta_dot_over_theta+Scalar(2)*beta)*p*w.transpose()
520
2
                         + wTp * beta                             * Matrix3::Identity()
521
2
                         + beta                                   *w*p.transpose());
522


2
        Jout.template topRightCorner<3,3>().noalias() -=
523
          - Jtmp3 * J;
524
2
        break;
525
      }
526
      default:
527
        assert(false && "Wrong Op requesed value");
528
        break;
529
      }
530
232
  }
531
532
  /// \brief Derivative of exp6
533
  /// Computed as the inverse of Jlog6
534
  template<typename MotionDerived, typename Matrix6Like>
535
60
  void Jexp6(const MotionDense<MotionDerived>     & nu,
536
             const Eigen::MatrixBase<Matrix6Like> & Jexp)
537
  {
538
60
    Jexp6<SETTO>(nu, Jexp);
539
60
  }
540
541
  /** \brief Derivative of log6
542
   *
543
   * This function is the right derivative of @ref log6, that is, for \f$M \in
544
   * SE(3)\f$ and \f$\xi in \mathfrak{se}(3)\f$, it provides the linear
545
   * approximation:
546
   *
547
   * \f[
548
   * \log_6(M \oplus \xi) = \log_6(M \exp_6(\xi)) \approx \log_6(M) + \text{Jlog6}(M) \xi
549
   * \f]
550
   *
551
   * Equivalently, \f$\text{Jlog6}\f$ is the right Jacobian of \f$\log_6\f$:
552
   *
553
   * \f[
554
   * \text{Jlog6}(M) = \frac{\partial \log_6(M)}{\partial M}
555
   * \f]
556
   *
557
   * Note that this is the right Jacobian: \f$\text{Jlog6}(M) : T_{M} SE(3) \to T_{\log_6(M)} \mathfrak{se}(3)\f$.
558
   * (By convention, calculations in Pinocchio always perform right differentiation,
559
   * i.e., Jacobians are in local coordinates (also known as body coordinates), unless otherwise specified.)
560
   *
561
   * Internally, it is calculated using the following formulas:
562
   *
563
   *  \f[
564
   *  \text{Jlog6}(M) =
565
   *  \left(\begin{array}{cc}
566
   *  \text{Jlog3}(R) & J * \text{Jlog3}(R) \\
567
   *            0     &     \text{Jlog3}(R) \\
568
   *  \end{array}\right)
569
   *  \f]
570
   *  where
571
   *  \f[ M =
572
   *  \left(\begin{array}{cc}
573
   *  \exp(\mathbf{r}) & \mathbf{p} \\
574
   *             0     & 1          \\
575
   *  \end{array}\right)
576
   *  \f]
577
   *  \f[
578
   *  \begin{eqnarray}
579
   *  J &=&
580
   *  \left.\frac{1}{2}[\mathbf{p}]_{\times} + \beta'(||r||) \frac{\mathbf{r}^T\mathbf{p}}{||r||}\mathbf{r}\mathbf{r}^T
581
   *  - (||r||\beta'(||r||) + 2 \beta(||r||)) \mathbf{p}\mathbf{r}^T\right.\\
582
   *  &&\left. + \mathbf{r}^T\mathbf{p}\beta(||r||)I_3 + \beta(||r||)\mathbf{r}\mathbf{p}^T\right.
583
   *  \end{eqnarray}
584
   *  \f]
585
   *  and
586
   *  \f[ \beta(x)=\left(\frac{1}{x^2} - \frac{\sin x}{2x(1-\cos x)}\right) \f]
587
   *
588
   *
589
   * \cheatsheet For \f$(A,B) \in SE(3)^2\f$, let \f$M_1(A, B) = A B\f$ and
590
   * \f$m_1 = \log_6(M_1) \f$. Then, we have the following partial (right)
591
   * Jacobians: \n
592
   *  - \f$ \frac{\partial m_1}{\partial A} = Jlog_6(M_1) Ad_B^{-1} \f$,
593
   *  - \f$ \frac{\partial m_1}{\partial B} = Jlog_6(M_1) \f$.
594
   *
595
   * \cheatsheet Let \f$A \in SE(3)\f$, \f$M_2(A) = A^{-1}\f$ and \f$m_2 =
596
   * \log_6(M_2)\f$. Then, we have the following partial (right) Jacobian: \n
597
   *  - \f$ \frac{\partial m_2}{\partial A} = - Jlog_6(M_2) Ad_A \f$.
598
   */
599
  template<typename Scalar, int Options, typename Matrix6Like>
600
315
  void Jlog6(const SE3Tpl<Scalar, Options> & M,
601
             const Eigen::MatrixBase<Matrix6Like> & Jlog)
602
  {
603
315
    Jlog6_impl<Scalar>::run(M,PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like,Jlog));
604
315
  }
605
606
  template<typename Scalar, int Options>
607
  template<typename OtherScalar>
608
26
  SE3Tpl<Scalar,Options> SE3Tpl<Scalar,Options>::Interpolate(const SE3Tpl & A,
609
                                                             const SE3Tpl & B,
610
                                                             const OtherScalar & alpha)
611
  {
612
    typedef SE3Tpl<Scalar,Options> ReturnType;
613
    typedef MotionTpl<Scalar,Options> Motion;
614
615

26
    Motion dv = log6(A.actInv(B));
616

26
    ReturnType res = A * exp6(alpha*dv);
617
52
    return res;
618
  }
619
620
} // namespace pinocchio
621
622
#include "pinocchio/spatial/explog-quaternion.hpp"
623
#include "pinocchio/spatial/log.hxx"
624
625
#endif //#ifndef __pinocchio_spatial_explog_hpp__