pinocchio  3.3.1
A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
explog.hpp
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 {
33  template<typename Vector3Like>
34  typename Eigen::
35  Matrix<typename Vector3Like::Scalar, 3, 3, PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like)::Options>
36  exp3(const Eigen::MatrixBase<Vector3Like> & v)
37  {
38  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  const Scalar t2 = v.squaredNorm() + eps * eps;
46 
47  const Scalar t = math::sqrt(t2);
48  Scalar ct, st;
49  SINCOS(t, &st, &ct);
50 
51  const Scalar alpha_vxvx = internal::if_then_else(
52  internal::GT, t, TaylorSeriesExpansion<Scalar>::template precision<3>(),
53  static_cast<Scalar>((1 - ct) / t2), static_cast<Scalar>(Scalar(1) / Scalar(2) - t2 / 24));
54  const Scalar alpha_vx = internal::if_then_else(
55  internal::GT, t, TaylorSeriesExpansion<Scalar>::template precision<3>(),
56  static_cast<Scalar>((st) / t), static_cast<Scalar>(Scalar(1) - t2 / 6));
57  Matrix3 res(alpha_vxvx * v * v.transpose());
58  res.coeffRef(0, 1) -= alpha_vx * v[2];
59  res.coeffRef(1, 0) += alpha_vx * v[2];
60  res.coeffRef(0, 2) += alpha_vx * v[1];
61  res.coeffRef(2, 0) -= alpha_vx * v[1];
62  res.coeffRef(1, 2) -= alpha_vx * v[0];
63  res.coeffRef(2, 1) += alpha_vx * v[0];
64 
65  ct = internal::if_then_else(
66  internal::GT, t, TaylorSeriesExpansion<Scalar>::template precision<3>(), ct,
67  static_cast<Scalar>(Scalar(1) - t2 / 2));
68  res.diagonal().array() += ct;
69 
70  return res;
71  }
72 
80  template<typename Matrix3Like>
81  Eigen::
82  Matrix<typename Matrix3Like::Scalar, 3, 1, PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix3Like)::Options>
83  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  Vector3 res;
88  log3_impl<Scalar>::run(R, theta, res);
89  return res;
90  }
91 
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 
117  template<AssignmentOperatorType op, typename Vector3Like, typename Matrix3Like>
118  void Jexp3(const Eigen::MatrixBase<Vector3Like> & r, const Eigen::MatrixBase<Matrix3Like> & Jexp)
119  {
120  PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE(Vector3Like, r, 3, 1);
121  PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix3Like, Jexp, 3, 3);
122 
123  Matrix3Like & Jout = PINOCCHIO_EIGEN_CONST_CAST(Matrix3Like, Jexp);
124  typedef typename Matrix3Like::Scalar Scalar;
125 
126  const Scalar n2 = r.squaredNorm();
127  const Scalar n = math::sqrt(n2);
128  const Scalar n_inv = Scalar(1) / n;
129  const Scalar n2_inv = n_inv * n_inv;
130  Scalar cn, sn;
131  SINCOS(n, &sn, &cn);
132 
133  const Scalar a = internal::if_then_else(
134  internal::LT, n, TaylorSeriesExpansion<Scalar>::template precision<3>(),
135  static_cast<Scalar>(Scalar(1) - n2 / Scalar(6)), static_cast<Scalar>(sn * n_inv));
136  const Scalar b = internal::if_then_else(
137  internal::LT, n, TaylorSeriesExpansion<Scalar>::template precision<3>(),
138  static_cast<Scalar>(-Scalar(1) / Scalar(2) - n2 / Scalar(24)),
139  static_cast<Scalar>(-(1 - cn) * n2_inv));
140  const Scalar c = internal::if_then_else(
141  internal::LT, n, TaylorSeriesExpansion<Scalar>::template precision<3>(),
142  static_cast<Scalar>(Scalar(1) / Scalar(6) - n2 / Scalar(120)),
143  static_cast<Scalar>(n2_inv * (1 - a)));
144 
145  switch (op)
146  {
147  case SETTO:
148  Jout.diagonal().setConstant(a);
149  Jout(0, 1) = -b * r[2];
150  Jout(1, 0) = -Jout(0, 1);
151  Jout(0, 2) = b * r[1];
152  Jout(2, 0) = -Jout(0, 2);
153  Jout(1, 2) = -b * r[0];
154  Jout(2, 1) = -Jout(1, 2);
155  Jout.noalias() += c * r * r.transpose();
156  break;
157  case ADDTO:
158  Jout.diagonal().array() += a;
159  Jout(0, 1) += -b * r[2];
160  Jout(1, 0) += b * r[2];
161  Jout(0, 2) += b * r[1];
162  Jout(2, 0) += -b * r[1];
163  Jout(1, 2) += -b * r[0];
164  Jout(2, 1) += b * r[0];
165  Jout.noalias() += c * r * r.transpose();
166  break;
167  case RMTO:
168  Jout.diagonal().array() -= a;
169  Jout(0, 1) -= -b * r[2];
170  Jout(1, 0) -= b * r[2];
171  Jout(0, 2) -= b * r[1];
172  Jout(2, 0) -= -b * r[1];
173  Jout(1, 2) -= -b * r[0];
174  Jout(2, 1) -= b * r[0];
175  Jout.noalias() -= c * r * r.transpose();
176  break;
177  default:
178  assert(false && "Wrong Op requesed value");
179  break;
180  }
181  }
182 
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 
239  template<typename Scalar, typename Vector3Like, typename Matrix3Like>
240  void Jlog3(
241  const Scalar & theta,
242  const Eigen::MatrixBase<Vector3Like> & log,
243  const Eigen::MatrixBase<Matrix3Like> & Jlog)
244  {
245  Jlog3_impl<Scalar>::run(theta, log, PINOCCHIO_EIGEN_CONST_CAST(Matrix3Like, Jlog));
246  }
247 
260  template<typename Matrix3Like1, typename Matrix3Like2>
261  void
262  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  Vector3 w(log3(R, t));
269  Jlog3(t, w, PINOCCHIO_EIGEN_CONST_CAST(Matrix3Like2, Jlog));
270  }
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 
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 
343  template<typename MotionDerived>
344  SE3Tpl<
345  typename MotionDerived::Scalar,
346  PINOCCHIO_EIGEN_PLAIN_TYPE(typename MotionDerived::Vector3)::Options>
347  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  SE3 res;
358  typename SE3::LinearType & trans = res.translation();
359  typename SE3::AngularType & rot = res.rotation();
360 
361  const typename MotionDerived::ConstAngularType & w = nu.angular();
362  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  const Scalar t2 = w.squaredNorm() + eps * eps;
367  const Scalar t = math::sqrt(t2);
368  Scalar ct, st;
369  SINCOS(t, &st, &ct);
370  const Scalar inv_t2 = Scalar(1) / t2;
371 
372  alpha_wxv = internal::if_then_else(
373  internal::LT, t, TaylorSeriesExpansion<Scalar>::template precision<3>(),
374  static_cast<Scalar>(Scalar(1) / Scalar(2) - t2 / 24),
375  static_cast<Scalar>((Scalar(1) - ct) * inv_t2));
376 
377  alpha_v = internal::if_then_else(
378  internal::LT, t, TaylorSeriesExpansion<Scalar>::template precision<3>(),
379  static_cast<Scalar>(Scalar(1) - t2 / 6), static_cast<Scalar>((st) / t));
380 
381  alpha_w = internal::if_then_else(
382  internal::LT, t, TaylorSeriesExpansion<Scalar>::template precision<3>(),
383  static_cast<Scalar>((Scalar(1) / Scalar(6) - t2 / 120)),
384  static_cast<Scalar>((Scalar(1) - alpha_v) * inv_t2));
385 
386  diagonal_term = internal::if_then_else(
387  internal::LT, t, TaylorSeriesExpansion<Scalar>::template precision<3>(),
388  static_cast<Scalar>(Scalar(1) - t2 / 2), ct);
389 
390  // Linear
391  trans.noalias() = (alpha_v * v + (alpha_w * w.dot(v)) * w + alpha_wxv * w.cross(v));
392 
393  // Rotational
394  rot.noalias() = alpha_wxv * w * w.transpose();
395  rot.coeffRef(0, 1) -= alpha_v * w[2];
396  rot.coeffRef(1, 0) += alpha_v * w[2];
397  rot.coeffRef(0, 2) += alpha_v * w[1];
398  rot.coeffRef(2, 0) -= alpha_v * w[1];
399  rot.coeffRef(1, 2) -= alpha_v * w[0];
400  rot.coeffRef(2, 1) += alpha_v * w[0];
401  rot.diagonal().array() += diagonal_term;
402 
403  return res;
404  }
405 
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 
434  template<typename Scalar, int Options>
435  MotionTpl<Scalar, Options> log6(const SE3Tpl<Scalar, Options> & M)
436  {
437  typedef MotionTpl<Scalar, Options> Motion;
438  Motion mout;
439  log6_impl<Scalar>::run(M, mout);
440  return mout;
441  }
442 
453  template<typename Vector3Like, typename QuaternionLike>
454  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  Motion mout;
460  log6_impl<Scalar>::run(quat, vec, mout);
461  return mout;
462  }
463 
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 
495  template<AssignmentOperatorType op, typename MotionDerived, typename Matrix6Like>
496  void Jexp6(const MotionDense<MotionDerived> & nu, const Eigen::MatrixBase<Matrix6Like> & Jexp)
497  {
498  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  Matrix6Like & Jout = PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like, Jexp);
504 
505  const typename MotionDerived::ConstLinearType & v = nu.linear();
506  const typename MotionDerived::ConstAngularType & w = nu.angular();
507  const Scalar t2 = w.squaredNorm();
508  const Scalar t = math::sqrt(t2);
509 
510  const Scalar tinv = Scalar(1) / t, t2inv = tinv * tinv;
511  Scalar st, ct;
512  SINCOS(t, &st, &ct);
513  const Scalar inv_2_2ct = Scalar(1) / (Scalar(2) * (Scalar(1) - ct));
514 
515  const Scalar beta = internal::if_then_else(
516  internal::LT, t, TaylorSeriesExpansion<Scalar>::template precision<3>(),
517  static_cast<Scalar>(Scalar(1) / Scalar(12) + t2 / Scalar(720)),
518  static_cast<Scalar>(t2inv - st * tinv * inv_2_2ct));
519 
520  const Scalar beta_dot_over_theta = internal::if_then_else(
521  internal::LT, t, TaylorSeriesExpansion<Scalar>::template precision<3>(),
522  static_cast<Scalar>(Scalar(1) / Scalar(360)),
523  static_cast<Scalar>(
524  -Scalar(2) * t2inv * t2inv + (Scalar(1) + st * tinv) * t2inv * inv_2_2ct));
525 
526  switch (op)
527  {
528  case SETTO: {
529  Jexp3<SETTO>(w, Jout.template bottomRightCorner<3, 3>());
530  Jout.template topLeftCorner<3, 3>() = Jout.template bottomRightCorner<3, 3>();
531  const Vector3 p = Jout.template topLeftCorner<3, 3>().transpose() * v;
532  const Scalar wTp(w.dot(p));
533  const Matrix3 J(
534  alphaSkew(.5, p) + (beta_dot_over_theta * wTp) * w * w.transpose()
535  - (t2 * beta_dot_over_theta + Scalar(2) * beta) * p * w.transpose()
536  + wTp * beta * Matrix3::Identity() + beta * w * p.transpose());
537  Jout.template topRightCorner<3, 3>().noalias() = -Jout.template topLeftCorner<3, 3>() * J;
538  Jout.template bottomLeftCorner<3, 3>().setZero();
539  break;
540  }
541  case ADDTO: {
542  PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
543  PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED
544  Matrix3 Jtmp3;
545  Jexp3<SETTO>(w, Jtmp3);
546  PINOCCHIO_COMPILER_DIAGNOSTIC_POP
547  Jout.template bottomRightCorner<3, 3>() += Jtmp3;
548  Jout.template topLeftCorner<3, 3>() += Jtmp3;
549  const Vector3 p = Jtmp3.transpose() * v;
550  const Scalar wTp(w.dot(p));
551  const Matrix3 J(
552  alphaSkew(.5, p) + (beta_dot_over_theta * wTp) * w * w.transpose()
553  - (t2 * beta_dot_over_theta + Scalar(2) * beta) * p * w.transpose()
554  + wTp * beta * Matrix3::Identity() + beta * w * p.transpose());
555  Jout.template topRightCorner<3, 3>().noalias() += -Jtmp3 * J;
556  break;
557  }
558  case RMTO: {
559  PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
560  PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED
561  Matrix3 Jtmp3;
562  Jexp3<SETTO>(w, Jtmp3);
563  PINOCCHIO_COMPILER_DIAGNOSTIC_POP
564  Jout.template bottomRightCorner<3, 3>() -= Jtmp3;
565  Jout.template topLeftCorner<3, 3>() -= Jtmp3;
566  const Vector3 p = Jtmp3.transpose() * v;
567  const Scalar wTp(w.dot(p));
568  const Matrix3 J(
569  alphaSkew(.5, p) + (beta_dot_over_theta * wTp) * w * w.transpose()
570  - (t2 * beta_dot_over_theta + Scalar(2) * beta) * p * w.transpose()
571  + wTp * beta * Matrix3::Identity() + beta * w * p.transpose());
572  Jout.template topRightCorner<3, 3>().noalias() -= -Jtmp3 * J;
573  break;
574  }
575  default:
576  assert(false && "Wrong Op requesed value");
577  break;
578  }
579  }
580 
583  template<typename MotionDerived, typename Matrix6Like>
584  void Jexp6(const MotionDense<MotionDerived> & nu, const Eigen::MatrixBase<Matrix6Like> & Jexp)
585  {
586  Jexp6<SETTO>(nu, Jexp);
587  }
588 
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 
667  template<typename Scalar, int Options, typename Matrix6Like>
668  void Jlog6(const SE3Tpl<Scalar, Options> & M, const Eigen::MatrixBase<Matrix6Like> & Jlog)
669  {
670  Jlog6_impl<Scalar>::run(M, PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like, Jlog));
671  }
672 
678  template<typename Scalar, int Options>
679  Eigen::Matrix<Scalar, 6, 6, Options> Jlog6(const SE3Tpl<Scalar, Options> & M)
680  {
681  typedef Eigen::Matrix<Scalar, 6, 6, Options> ReturnType;
682 
683  ReturnType res;
684  Jlog6(M, res);
685  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__
pinocchio
Main pinocchio namespace.
Definition: treeview.dox:11
pinocchio::Jexp3
void Jexp3(const Eigen::MatrixBase< Vector3Like > &r, const Eigen::MatrixBase< Matrix3Like > &Jexp)
Derivative of .
Definition: explog.hpp:118
pinocchio::exp3
Eigen::Matrix< typename Vector3Like::Scalar, 3, 3, Vector3Like ::Options > exp3(const Eigen::MatrixBase< Vector3Like > &v)
Exp: so3 -> SO3.
Definition: explog.hpp:36
pinocchio::Jexp6
void Jexp6(const MotionDense< MotionDerived > &nu, const Eigen::MatrixBase< Matrix6Like > &Jexp)
Derivative of exp6 Computed as the inverse of Jlog6.
Definition: explog.hpp:496
pinocchio::log6
MotionTpl< Scalar, Options > log6(const SE3Tpl< Scalar, Options > &M)
Log: SE3 -> se3.
Definition: explog.hpp:435
pinocchio::exp6
SE3Tpl< typename MotionDerived::Scalar, typename MotionDerived::Vector3 ::Options > exp6(const MotionDense< MotionDerived > &nu)
Exp: se3 -> SE3.
Definition: explog.hpp:347
pinocchio::Jlog3
void Jlog3(const Scalar &theta, const Eigen::MatrixBase< Vector3Like > &log, const Eigen::MatrixBase< Matrix3Like > &Jlog)
Derivative of log3.
Definition: explog.hpp:240
pinocchio::addSkew
void addSkew(const Eigen::MatrixBase< Vector3Like > &v, const Eigen::MatrixBase< Matrix3Like > &M)
Add skew matrix represented by a 3d vector to a given matrix, i.e. add the antisymmetric matrix repre...
Definition: skew.hpp:68
pinocchio::log3
Eigen::Matrix< typename Matrix3Like::Scalar, 3, 1, Matrix3Like ::Options > log3(const Eigen::MatrixBase< Matrix3Like > &R, typename Matrix3Like::Scalar &theta)
Same as log3.
Definition: explog.hpp:83
pinocchio::Jlog6
void Jlog6(const SE3Tpl< Scalar, Options > &M, const Eigen::MatrixBase< Matrix6Like > &Jlog)
Derivative of log6.
Definition: explog.hpp:668
pinocchio::SINCOS
void SINCOS(const S1 &a, S2 *sa, S3 *ca)
Computes sin/cos values of a given input scalar.
Definition: sincos.hpp:27
pinocchio::alphaSkew
void alphaSkew(const Scalar alpha, const Eigen::MatrixBase< Vector3 > &v, const Eigen::MatrixBase< Matrix3 > &M)
Computes the skew representation of a given 3d vector multiplied by a given scalar....
Definition: skew.hpp:134
pinocchio::Jlog3_impl
Definition: log.hpp:13
pinocchio::Jlog6_impl
Definition: log.hpp:18
pinocchio::SE3Tpl::Interpolate
static SE3Tpl Interpolate(const SE3Tpl &A, const SE3Tpl &B, const OtherScalar &alpha)
Linear interpolation on the SE3 manifold.
pinocchio::log3_impl
Definition: log.hpp:11
pinocchio::log6_impl
Definition: log.hpp:16