pinocchio  2.4.0-dirty
A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
explog.hpp
1 //
2 // Copyright (c) 2015-2020 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 {
32  template<typename Vector3Like>
33  typename Eigen::Matrix<typename Vector3Like::Scalar,3,3,PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like)::Options>
34  exp3(const Eigen::MatrixBase<Vector3Like> & v)
35  {
36  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  const Scalar t2 = v.squaredNorm();
43 
44  const Scalar t = math::sqrt(t2);
45  if(t > TaylorSeriesExpansion<Scalar>::template precision<3>())
46  {
47  Scalar ct,st; SINCOS(t,&st,&ct);
48  const Scalar alpha_vxvx = (1 - ct)/t2;
49  const Scalar alpha_vx = (st)/t;
50  Matrix3 res(alpha_vxvx * v * v.transpose());
51  res.coeffRef(0,1) -= alpha_vx * v[2]; res.coeffRef(1,0) += alpha_vx * v[2];
52  res.coeffRef(0,2) += alpha_vx * v[1]; res.coeffRef(2,0) -= alpha_vx * v[1];
53  res.coeffRef(1,2) -= alpha_vx * v[0]; res.coeffRef(2,1) += alpha_vx * v[0];
54  res.diagonal().array() += ct;
55 
56  return res;
57  }
58  else
59  {
60  const Scalar alpha_vxvx = Scalar(1)/Scalar(2) - t2/24;
61  const Scalar alpha_vx = Scalar(1) - t2/6;
62  Matrix3 res(alpha_vxvx * v * v.transpose());
63  res.coeffRef(0,1) -= alpha_vx * v[2]; res.coeffRef(1,0) += alpha_vx * v[2];
64  res.coeffRef(0,2) += alpha_vx * v[1]; res.coeffRef(2,0) -= alpha_vx * v[1];
65  res.coeffRef(1,2) -= alpha_vx * v[0]; res.coeffRef(2,1) += alpha_vx * v[0];
66  res.diagonal().array() += Scalar(1) - t2/2;
67 
68  return res;
69  }
70  }
71 
79  template<typename Matrix3Like>
80  Eigen::Matrix<typename Matrix3Like::Scalar,3,1,PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix3Like)::Options>
81  log3(const Eigen::MatrixBase<Matrix3Like> & R,
82  typename Matrix3Like::Scalar & theta)
83  {
84  typedef typename Matrix3Like::Scalar Scalar;
85  typedef Eigen::Matrix<Scalar,3,1,
86  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::Matrix<typename Matrix3Like::Scalar,3,1,PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix3Like)::Options>
102  log3(const Eigen::MatrixBase<Matrix3Like> & R)
103  {
104  typename Matrix3Like::Scalar theta;
105  return log3(R.derived(),theta);
106  }
107 
116  template<typename Vector3Like, typename Matrix3Like>
117  void Jexp3(const Eigen::MatrixBase<Vector3Like> & r,
118  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  Scalar n2 = r.squaredNorm(),a,b,c;
127  Scalar n = math::sqrt(n2);
128 
129  if (n < TaylorSeriesExpansion<Scalar>::template precision<3>())
130  {
131  a = Scalar(1) - n2/Scalar(6);
132  b = - Scalar(1)/Scalar(2) - n2/Scalar(24);
133  c = Scalar(1)/Scalar(6) - n2/Scalar(120);
134  }
135  else
136  {
137  Scalar n_inv = Scalar(1)/n;
138  Scalar n2_inv = n_inv * n_inv;
139  Scalar cn,sn; SINCOS(n,&sn,&cn);
140 
141  a = sn*n_inv;
142  b = - (1-cn)*n2_inv;
143  c = n2_inv * (1 - a);
144  }
145 
146  Jout.diagonal().setConstant(a);
147 
148  Jout(0,1) = -b*r[2]; Jout(1,0) = -Jout(0,1);
149  Jout(0,2) = b*r[1]; Jout(2,0) = -Jout(0,2);
150  Jout(1,2) = -b*r[0]; Jout(2,1) = -Jout(1,2);
151 
152  Jout.noalias() += c * r * r.transpose();
153  }
154 
167  template<typename Scalar, typename Vector3Like, typename Matrix3Like>
168  void Jlog3(const Scalar & theta,
169  const Eigen::MatrixBase<Vector3Like> & log,
170  const Eigen::MatrixBase<Matrix3Like> & Jlog)
171  {
172  Jlog3_impl<Scalar>::run(theta, log,
173  PINOCCHIO_EIGEN_CONST_CAST(Matrix3Like,Jlog));
174  }
175 
188  template<typename Matrix3Like1, typename Matrix3Like2>
189  void Jlog3(const Eigen::MatrixBase<Matrix3Like1> & R,
190  const Eigen::MatrixBase<Matrix3Like2> & Jlog)
191  {
192  typedef typename Matrix3Like1::Scalar Scalar;
193  typedef Eigen::Matrix<Scalar,3,1,PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix3Like1)::Options> Vector3;
194 
195  Scalar t;
196  Vector3 w(log3(R,t));
197  Jlog3(t,w,PINOCCHIO_EIGEN_CONST_CAST(Matrix3Like2,Jlog));
198  }
199 
209  template<typename MotionDerived>
212  {
213  typedef typename MotionDerived::Scalar Scalar;
214  enum { Options = PINOCCHIO_EIGEN_PLAIN_TYPE(typename MotionDerived::Vector3)::Options };
215 
216  typedef SE3Tpl<Scalar,Options> SE3;
217 
218  SE3 res;
219  typename SE3::LinearType & trans = res.translation();
220  typename SE3::AngularType & rot = res.rotation();
221 
222  const typename MotionDerived::ConstAngularType & w = nu.angular();
223  const typename MotionDerived::ConstLinearType & v = nu.linear();
224 
225  Scalar alpha_wxv, alpha_v, alpha_w, diagonal_term;
226  const Scalar t2 = w.squaredNorm();
227  const Scalar t = math::sqrt(t2);
228  Scalar ct,st; SINCOS(t,&st,&ct);
229  const Scalar inv_t2 = Scalar(1)/t2;
230 
231  alpha_wxv = internal::if_then_else(t<TaylorSeriesExpansion<Scalar>::template precision<3>(),
232  Scalar(1)/Scalar(2) - t2/24,
233  (Scalar(1) - ct)*inv_t2);
234 
235  alpha_v = internal::if_then_else(t<TaylorSeriesExpansion<Scalar>::template precision<3>(),
236  Scalar(1) - t2/6,
237  (st)/t);
238 
239  alpha_w = internal::if_then_else(t<TaylorSeriesExpansion<Scalar>::template precision<3>(),
240  (Scalar(1)/Scalar(6) - t2/120),
241  (Scalar(1) - alpha_v)*inv_t2);
242 
243  diagonal_term = internal::if_then_else(t<TaylorSeriesExpansion<Scalar>::template precision<3>(),
244  Scalar(1) - t2/2,
245  ct);
246 
247  // Linear
248  trans.noalias() = (alpha_v*v + (alpha_w*w.dot(v))*w + alpha_wxv*w.cross(v));
249 
250  // Rotational
251  rot.noalias() = alpha_wxv * w * w.transpose();
252  rot.coeffRef(0,1) -= alpha_v * w[2]; rot.coeffRef(1,0) += alpha_v * w[2];
253  rot.coeffRef(0,2) += alpha_v * w[1]; rot.coeffRef(2,0) -= alpha_v * w[1];
254  rot.coeffRef(1,2) -= alpha_v * w[0]; rot.coeffRef(2,1) += alpha_v * w[0];
255  rot.diagonal().array() += diagonal_term;
256 
257  return res;
258  }
259 
268  template<typename Vector6Like>
270  exp6(const Eigen::MatrixBase<Vector6Like> & v)
271  {
272  PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE (Vector6Like, v, 6, 1);
273 
274  MotionRef<const Vector6Like> nu(v.derived());
275  return exp6(nu);
276  }
277 
286  template<typename Scalar, int Options>
289  {
291  Motion mout;
292  log6_impl<Scalar>::run(M, mout);
293  return mout;
294  }
295 
304  template<typename Matrix4Like>
306  log6(const Eigen::MatrixBase<Matrix4Like> & M)
307  {
308  PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix4Like, M, 4, 4);
309 
310  typedef typename Matrix4Like::Scalar Scalar;
311  enum {Options = Eigen::internal::traits<Matrix4Like>::Options};
313  typedef SE3Tpl<Scalar,Options> SE3;
314 
315  SE3 m(M);
316  Motion mout;
317  log6_impl<Scalar>::run(m, mout);
318  return mout;
319  }
320 
323  template<typename MotionDerived, typename Matrix6Like>
325  const Eigen::MatrixBase<Matrix6Like> & Jexp)
326  {
327  PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE (Matrix6Like, Jexp, 6, 6);
328 
329  typedef typename MotionDerived::Scalar Scalar;
330  typedef typename MotionDerived::Vector3 Vector3;
331  typedef Eigen::Matrix<Scalar, 3, 3, Vector3::Options> Matrix3;
332  Matrix6Like & Jout = PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like,Jexp);
333 
334  const typename MotionDerived::ConstLinearType & v = nu.linear();
335  const typename MotionDerived::ConstAngularType & w = nu.angular();
336  const Scalar t2 = w.squaredNorm();
337  const Scalar t = math::sqrt(t2);
338 
339  // Matrix3 J3;
340  // Jexp3(w, J3);
341  Jexp3(w, Jout.template bottomRightCorner<3,3>());
342  Jout.template topLeftCorner<3,3>() = Jout.template bottomRightCorner<3,3>();
343 
344  Scalar beta, beta_dot_over_theta;
345  if (t < TaylorSeriesExpansion<Scalar>::template precision<3>())
346  {
347  beta = Scalar(1)/Scalar(12) + t2/Scalar(720);
348  beta_dot_over_theta = Scalar(1)/Scalar(360);
349  }
350  else
351  {
352  const Scalar tinv = Scalar(1)/t,
353  t2inv = tinv*tinv;
354  Scalar st,ct; SINCOS (t, &st, &ct);
355  const Scalar inv_2_2ct = Scalar(1)/(Scalar(2)*(Scalar(1)-ct));
356 
357  beta = t2inv - st*tinv*inv_2_2ct;
358  beta_dot_over_theta = -Scalar(2)*t2inv*t2inv +
359  (Scalar(1) + st*tinv) * t2inv * inv_2_2ct;
360  }
361 
362  Vector3 p (Jout.template topLeftCorner<3,3>().transpose() * v);
363  Scalar wTp (w.dot (p));
364  Matrix3 J (alphaSkew(.5, p) +
365  (beta_dot_over_theta*wTp) *w*w.transpose()
366  - (t2*beta_dot_over_theta+Scalar(2)*beta)*p*w.transpose()
367  + wTp * beta * Matrix3::Identity()
368  + beta *w*p.transpose());
369 
370  Jout.template topRightCorner<3,3>().noalias() =
371  - Jout.template topLeftCorner<3,3>() * J;
372  Jout.template bottomLeftCorner<3,3>().setZero();
373  }
374 
395  template<typename Scalar, int Options, typename Matrix6Like>
397  const Eigen::MatrixBase<Matrix6Like> & Jlog)
398  {
399  Jlog6_impl<Scalar>::run(M,PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like,Jlog));
400  }
401 
402  template<typename Scalar, int Options>
403  template<typename OtherScalar>
405  const SE3Tpl & B,
406  const OtherScalar & alpha)
407  {
408  typedef SE3Tpl<Scalar,Options> ReturnType;
410 
411  Motion dv = log6(A.actInv(B));
412  ReturnType res = A * exp6(alpha*dv);
413  return res;
414  }
415 
416 } // namespace pinocchio
417 
418 #include "pinocchio/spatial/explog-quaternion.hpp"
419 #include "pinocchio/spatial/log.hxx"
420 
421 #endif //#ifndef __pinocchio_spatial_explog_hpp__
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:81
void Jlog3(const Scalar &theta, const Eigen::MatrixBase< Vector3Like > &log, const Eigen::MatrixBase< Matrix3Like > &Jlog)
Derivative of log3.
Definition: explog.hpp:168
Eigen::Matrix< typename Vector3Like::Scalar, 3, 3, Vector3Like ::Options > exp3(const Eigen::MatrixBase< Vector3Like > &v)
Exp: so3 -> SO3.
Definition: explog.hpp:34
void Jlog6(const SE3Tpl< Scalar, Options > &M, const Eigen::MatrixBase< Matrix6Like > &Jlog)
Derivative of log6 where and .
Definition: explog.hpp:396
MotionTpl< Scalar, Options > log6(const SE3Tpl< Scalar, Options > &M)
Log: SE3 -> se3.
Definition: explog.hpp:288
void Jexp3(const Eigen::MatrixBase< Vector3Like > &r, const Eigen::MatrixBase< Matrix3Like > &Jexp)
Derivative of .
Definition: explog.hpp:117
void Jexp6(const MotionDense< MotionDerived > &nu, const Eigen::MatrixBase< Matrix6Like > &Jexp)
Derivative of exp6 Computed as the inverse of Jlog6.
Definition: explog.hpp:324
void SINCOS(const S1 &a, S2 *sa, S3 *ca)
Computes sin/cos values of a given input scalar.
Definition: sincos.hpp:26
Main pinocchio namespace.
Definition: treeview.dox:24
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. i.e. the antisymmetric matrix representation of the cross product operator ( )
Definition: skew.hpp:124
SE3Tpl< typename MotionDerived::Scalar, typename MotionDerived::Vector3 ::Options > exp6(const MotionDense< MotionDerived > &nu)
Exp: se3 -> SE3.
Definition: explog.hpp:211
SE3GroupAction< D >::ReturnType actInv(const D &d) const
by = aXb.actInv(ay)
Definition: se3-base.hpp:97
static SE3Tpl Interpolate(const SE3Tpl &A, const SE3Tpl &B, const OtherScalar &alpha)
Linear interpolation on the SE3 manifold.