pinocchio  2.2.1-dirty
explog.hpp
1 //
2 // Copyright (c) 2015-2019 CNRS INRIA
3 // Copyright (c) 2015 Wandercraft, 86 rue de Paris 91400 Orsay, France.
4 //
5 
6 #ifndef __spatial_explog_hpp__
7 #define __spatial_explog_hpp__
8 
9 #include <Eigen/Geometry>
10 
11 #include "pinocchio/fwd.hpp"
12 #include "pinocchio/utils/static-if.hpp"
13 #include "pinocchio/math/fwd.hpp"
14 #include "pinocchio/math/sincos.hpp"
15 #include "pinocchio/math/taylor-expansion.hpp"
16 #include "pinocchio/spatial/motion.hpp"
17 #include "pinocchio/spatial/skew.hpp"
18 #include "pinocchio/spatial/se3.hpp"
19 
20 namespace pinocchio
21 {
30  template<typename Vector3Like>
31  typename Eigen::Matrix<typename Vector3Like::Scalar,3,3,PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like)::Options>
32  exp3(const Eigen::MatrixBase<Vector3Like> & v)
33  {
34  PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE (Vector3Like, v, 3, 1);
35 
36  typedef typename Vector3Like::Scalar Scalar;
37  typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like) Vector3LikePlain;
38  typedef Eigen::Matrix<Scalar,3,3,Vector3LikePlain::Options> Matrix3;
39 
40  const Scalar t2 = v.squaredNorm();
41 
42  const Scalar t = math::sqrt(t2);
43  if(t > TaylorSeriesExpansion<Scalar>::template precision<3>())
44  {
45  Scalar ct,st; SINCOS(t,&st,&ct);
46  const Scalar alpha_vxvx = (1 - ct)/t2;
47  const Scalar alpha_vx = (st)/t;
48  Matrix3 res(alpha_vxvx * v * v.transpose());
49  res.coeffRef(0,1) -= alpha_vx * v[2]; res.coeffRef(1,0) += alpha_vx * v[2];
50  res.coeffRef(0,2) += alpha_vx * v[1]; res.coeffRef(2,0) -= alpha_vx * v[1];
51  res.coeffRef(1,2) -= alpha_vx * v[0]; res.coeffRef(2,1) += alpha_vx * v[0];
52  res.diagonal().array() += ct;
53 
54  return res;
55  }
56  else
57  {
58  const Scalar alpha_vxvx = Scalar(1)/Scalar(2) - t2/24;
59  const Scalar alpha_vx = Scalar(1) - t2/6;
60  Matrix3 res(alpha_vxvx * v * v.transpose());
61  res.coeffRef(0,1) -= alpha_vx * v[2]; res.coeffRef(1,0) += alpha_vx * v[2];
62  res.coeffRef(0,2) += alpha_vx * v[1]; res.coeffRef(2,0) -= alpha_vx * v[1];
63  res.coeffRef(1,2) -= alpha_vx * v[0]; res.coeffRef(2,1) += alpha_vx * v[0];
64  res.diagonal().array() += Scalar(1) - t2/2;
65 
66  return res;
67  }
68  }
69 
77  template<typename Matrix3Like>
78  Eigen::Matrix<typename Matrix3Like::Scalar,3,1,PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix3Like)::Options>
79  log3(const Eigen::MatrixBase<Matrix3Like> & R,
80  typename Matrix3Like::Scalar & theta)
81  {
82  PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix3Like, R, 3, 3);
83 
84  typedef typename Matrix3Like::Scalar Scalar;
85  typedef Eigen::Matrix<Scalar,3,1,PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix3Like)::Options> Vector3;
86 
87  static const Scalar PI_value = PI<Scalar>();
88 
89  Vector3 res;
90  const Scalar tr = R.trace();
91  if(tr > Scalar(3)) theta = 0; // acos((3-1)/2)
92  else if(tr < Scalar(-1)) theta = PI_value; // acos((-1-1)/2)
93  else theta = math::acos((tr - Scalar(1))/Scalar(2));
94  assert(theta == theta && "theta contains some NaN"); // theta != NaN
95 
96  // From runs of hpp-constraints/tests/logarithm.cc: 1e-6 is too small.
97  if(theta >= PI_value - 1e-2)
98  {
99  // 1e-2: A low value is not required since the computation is
100  // using explicit formula. However, the precision of this method
101  // is the square root of the precision with the antisymmetric
102  // method (Nominal case).
103  const Scalar cphi = cos(theta - PI_value);
104  const Scalar beta = theta*theta / ( Scalar(1) + cphi );
105  Vector3 tmp((R.diagonal().array() + cphi) * beta);
106  res(0) = (R (2, 1) > R (1, 2) ? Scalar(1) : Scalar(-1)) * (tmp[0] > Scalar(0) ? sqrt(tmp[0]) : Scalar(0));
107  res(1) = (R (0, 2) > R (2, 0) ? Scalar(1) : Scalar(-1)) * (tmp[1] > Scalar(0) ? sqrt(tmp[1]) : Scalar(0));
108  res(2) = (R (1, 0) > R (0, 1) ? Scalar(1) : Scalar(-1)) * (tmp[2] > Scalar(0) ? sqrt(tmp[2]) : Scalar(0)); }
109  else
110  {
111  const Scalar t = ((theta > TaylorSeriesExpansion<Scalar>::template precision<3>())
112  ? theta / sin(theta)
113  : Scalar(1)) / Scalar(2);
114  res(0) = t * (R (2, 1) - R (1, 2));
115  res(1) = t * (R (0, 2) - R (2, 0));
116  res(2) = t * (R (1, 0) - R (0, 1));
117 
118  }
119 
120  return res;
121  }
122 
131  template<typename Matrix3Like>
132  Eigen::Matrix<typename Matrix3Like::Scalar,3,1,PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix3Like)::Options>
133  log3(const Eigen::MatrixBase<Matrix3Like> & R)
134  {
135  PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE (Matrix3Like, R, 3, 3);
136 
137  typename Matrix3Like::Scalar theta;
138  return log3(R.derived(),theta);
139  }
140 
149  template<typename Vector3Like, typename Matrix3Like>
150  void Jexp3(const Eigen::MatrixBase<Vector3Like> & r,
151  const Eigen::MatrixBase<Matrix3Like> & Jexp)
152  {
153  PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE (Vector3Like, r , 3, 1);
154  PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE (Matrix3Like, Jexp, 3, 3);
155 
156  Matrix3Like & Jout = PINOCCHIO_EIGEN_CONST_CAST(Matrix3Like,Jexp);
157  typedef typename Matrix3Like::Scalar Scalar;
158 
159  Scalar n2 = r.squaredNorm(),a,b,c;
160  Scalar n = math::sqrt(n2);
161 
162  if (n < TaylorSeriesExpansion<Scalar>::template precision<3>())
163  {
164  a = Scalar(1) - n2/Scalar(6);
165  b = - Scalar(1)/Scalar(2) - n2/Scalar(24);
166  c = Scalar(1)/Scalar(6) - n2/Scalar(120);
167  }
168  else
169  {
170  Scalar n_inv = Scalar(1)/n;
171  Scalar n2_inv = n_inv * n_inv;
172  Scalar cn,sn; SINCOS(n,&sn,&cn);
173 
174  a = sn*n_inv;
175  b = - (1-cn)*n2_inv;
176  c = n2_inv * (1 - a);
177  }
178 
179  Jout.diagonal().setConstant(a);
180 
181  Jout(0,1) = -b*r[2]; Jout(1,0) = -Jout(0,1);
182  Jout(0,2) = b*r[1]; Jout(2,0) = -Jout(0,2);
183  Jout(1,2) = -b*r[0]; Jout(2,1) = -Jout(1,2);
184 
185  Jout.noalias() += c * r * r.transpose();
186  }
187 
200  template<typename Scalar, typename Vector3Like, typename Matrix3Like>
201  void Jlog3(const Scalar & theta,
202  const Eigen::MatrixBase<Vector3Like> & log,
203  const Eigen::MatrixBase<Matrix3Like> & Jlog)
204  {
205  PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE (Vector3Like, log, 3, 1);
206  PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE (Matrix3Like, Jlog, 3, 3);
207 
208  Matrix3Like & Jout = PINOCCHIO_EIGEN_CONST_CAST(Matrix3Like,Jlog);
209 
210  if (theta < TaylorSeriesExpansion<Scalar>::template precision<3>())
211  {
212  const Scalar alpha = Scalar(1)/Scalar(12) + theta*theta / Scalar(720);
213  Jout.noalias() = alpha * log * log.transpose();
214 
215  Jout.diagonal().array() += Scalar(0.5) * (2 - theta*theta / Scalar(6));
216 
217  // Jlog += r_{\times}/2
218  addSkew(0.5 * log, Jlog);
219  }
220  else
221  {
222  // Jlog = alpha I
223  Scalar ct,st; SINCOS(theta,&st,&ct);
224  const Scalar st_1mct = st/(Scalar(1)-ct);
225 
226  const Scalar alpha = Scalar(1)/(theta*theta) - st_1mct/(Scalar(2)*theta);
227  Jout.noalias() = alpha * log * log.transpose();
228 
229  Jout.diagonal().array() += Scalar(0.5) * (theta*st_1mct);
230 
231  // Jlog += r_{\times}/2
232  addSkew(0.5 * log, Jlog);
233  }
234  }
235 
248  template<typename Matrix3Like1, typename Matrix3Like2>
249  void Jlog3(const Eigen::MatrixBase<Matrix3Like1> & R,
250  const Eigen::MatrixBase<Matrix3Like2> & Jlog)
251  {
252  PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE (Matrix3Like1, R, 3, 3);
253  PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE (Matrix3Like2, Jlog, 3, 3);
254 
255  typedef typename Matrix3Like1::Scalar Scalar;
256  typedef Eigen::Matrix<Scalar,3,1,PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix3Like1)::Options> Vector3;
257 
258  Scalar t;
259  Vector3 w(log3(R,t));
260  Jlog3(t,w,PINOCCHIO_EIGEN_CONST_CAST(Matrix3Like2,Jlog));
261  }
262 
272  template<typename MotionDerived>
275  {
276  typedef typename MotionDerived::Scalar Scalar;
277  enum { Options = PINOCCHIO_EIGEN_PLAIN_TYPE(typename MotionDerived::Vector3)::Options };
278 
279  typedef SE3Tpl<Scalar,Options> SE3;
280 
281  SE3 res;
282  typename SE3::LinearType & trans = res.translation();
283  typename SE3::AngularType & rot = res.rotation();
284 
285  const typename MotionDerived::ConstAngularType & w = nu.angular();
286  const typename MotionDerived::ConstLinearType & v = nu.linear();
287 
288  Scalar alpha_wxv, alpha_v, alpha_w, diagonal_term;
289  const Scalar t2 = w.squaredNorm();
290  const Scalar t = math::sqrt(t2);
291  Scalar ct,st; SINCOS(t,&st,&ct);
292  const Scalar inv_t2 = Scalar(1)/t2;
293 
294  alpha_wxv = internal::if_then_else(t<TaylorSeriesExpansion<Scalar>::template precision<3>(),
295  Scalar(1)/Scalar(2) - t2/24,
296  (Scalar(1) - ct)*inv_t2);
297 
298  alpha_v = internal::if_then_else(t<TaylorSeriesExpansion<Scalar>::template precision<3>(),
299  Scalar(1) - t2/6,
300  (st)/t);
301 
302  alpha_w = internal::if_then_else(t<TaylorSeriesExpansion<Scalar>::template precision<3>(),
303  (Scalar(1)/Scalar(6) - t2/120),
304  (Scalar(1) - alpha_v)*inv_t2);
305 
306  diagonal_term = internal::if_then_else(t<TaylorSeriesExpansion<Scalar>::template precision<3>(),
307  Scalar(1) - t2/2,
308  ct);
309 
310  // Linear
311  trans.noalias() = (alpha_v*v + (alpha_w*w.dot(v))*w + alpha_wxv*w.cross(v));
312 
313  // Rotational
314  rot.noalias() = alpha_wxv * w * w.transpose();
315  rot.coeffRef(0,1) -= alpha_v * w[2]; rot.coeffRef(1,0) += alpha_v * w[2];
316  rot.coeffRef(0,2) += alpha_v * w[1]; rot.coeffRef(2,0) -= alpha_v * w[1];
317  rot.coeffRef(1,2) -= alpha_v * w[0]; rot.coeffRef(2,1) += alpha_v * w[0];
318  rot.diagonal().array() += diagonal_term;
319 
320  return res;
321  }
322 
331  template<typename Vector6Like>
333  exp6(const Eigen::MatrixBase<Vector6Like> & v)
334  {
335  PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE (Vector6Like, v, 6, 1);
336 
337  MotionRef<const Vector6Like> nu(v.derived());
338  return exp6(nu);
339  }
340 
349  template <typename Scalar, int Options>
352  {
353  typedef SE3Tpl<Scalar,Options> SE3;
355  typedef typename SE3::Vector3 Vector3;
356 
357  typename SE3::ConstAngularRef R = M.rotation();
358  typename SE3::ConstLinearRef p = M.translation();
359 
360  Scalar t;
361  Vector3 w(log3(R,t)); // t in [0,π]
362  const Scalar t2 = t*t;
363  Scalar alpha, beta;
364  if (t < TaylorSeriesExpansion<Scalar>::template precision<3>())
365  {
366  alpha = Scalar(1) - t2/Scalar(12) - t2*t2/Scalar(720);
367  beta = Scalar(1)/Scalar(12) + t2/Scalar(720);
368  }
369  else
370  {
371  Scalar st,ct; SINCOS(t,&st,&ct);
372  alpha = t*st/(Scalar(2)*(Scalar(1)-ct));
373  beta = Scalar(1)/t2 - st/(Scalar(2)*t*(Scalar(1)-ct));
374  }
375 
376  return Motion(alpha * p - 0.5 * w.cross(p) + beta * w.dot(p) * w,
377  w);
378  }
379 
388  template<typename Matrix4Like>
390  log6(const Eigen::MatrixBase<Matrix4Like> & M)
391  {
392  PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE (Matrix4Like, M, 4, 4);
393 
395  return log6(m);
396  }
397 
400  template<typename MotionDerived, typename Matrix6Like>
402  const Eigen::MatrixBase<Matrix6Like> & Jexp)
403  {
404  PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE (Matrix6Like, Jexp, 6, 6);
405 
406  typedef typename MotionDerived::Scalar Scalar;
407  typedef typename MotionDerived::Vector3 Vector3;
408  typedef Eigen::Matrix<Scalar, 3, 3, Vector3::Options> Matrix3;
409  Matrix6Like & Jout = PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like,Jexp);
410 
411  const typename MotionDerived::ConstLinearType & v = nu.linear();
412  const typename MotionDerived::ConstAngularType & w = nu.angular();
413  const Scalar t2 = w.squaredNorm();
414  const Scalar t = math::sqrt(t2);
415 
416  // Matrix3 J3;
417  // Jexp3(w, J3);
418  Jexp3(w, Jout.template bottomRightCorner<3,3>());
419  Jout.template topLeftCorner<3,3>() = Jout.template bottomRightCorner<3,3>();
420 
421  Scalar beta, beta_dot_over_theta;
422  if (t < TaylorSeriesExpansion<Scalar>::template precision<3>())
423  {
424  beta = Scalar(1)/Scalar(12) + t2/Scalar(720);
425  beta_dot_over_theta = Scalar(1)/Scalar(360);
426  }
427  else
428  {
429  const Scalar tinv = Scalar(1)/t,
430  t2inv = tinv*tinv;
431  Scalar st,ct; SINCOS (t, &st, &ct);
432  const Scalar inv_2_2ct = Scalar(1)/(Scalar(2)*(Scalar(1)-ct));
433 
434  beta = t2inv - st*tinv*inv_2_2ct;
435  beta_dot_over_theta = -Scalar(2)*t2inv*t2inv +
436  (Scalar(1) + st*tinv) * t2inv * inv_2_2ct;
437  }
438 
439  Vector3 p (Jout.template topLeftCorner<3,3>().transpose() * v);
440  Scalar wTp (w.dot (p));
441  Matrix3 J (alphaSkew(.5, p) +
442  (beta_dot_over_theta*wTp) *w*w.transpose()
443  - (t2*beta_dot_over_theta+Scalar(2)*beta)*p*w.transpose()
444  + wTp * beta * Matrix3::Identity()
445  + beta *w*p.transpose());
446 
447  Jout.template topRightCorner<3,3>().noalias() =
448  - Jout.template topLeftCorner<3,3>() * J;
449  Jout.template bottomLeftCorner<3,3>().setZero();
450  }
451 
472  template<typename Scalar, int Options, typename Matrix6Like>
474  const Eigen::MatrixBase<Matrix6Like> & Jlog)
475  {
476  PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE (Matrix6Like, Jlog, 6, 6);
477 
478  typedef SE3Tpl<Scalar,Options> SE3;
479  typedef typename SE3::Vector3 Vector3;
480  Matrix6Like & value = PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like,Jlog);
481 
482  typename SE3::ConstAngularRef R = M.rotation();
483  typename SE3::ConstLinearRef p = M.translation();
484 
485  Scalar t;
486  Vector3 w(log3(R,t));
487 
488  // value is decomposed as following:
489  // value = [ A, B;
490  // C, D ]
491  typedef Eigen::Block<Matrix6Like,3,3> Block33;
492  Block33 A = value.template topLeftCorner<3,3>();
493  Block33 B = value.template topRightCorner<3,3>();
494  Block33 C = value.template bottomLeftCorner<3,3>();
495  Block33 D = value.template bottomRightCorner<3,3>();
496 
497  Jlog3(t, w, A);
498  D = A;
499 
500  const Scalar t2 = t*t;
501  Scalar beta, beta_dot_over_theta;
502  if(t < TaylorSeriesExpansion<Scalar>::template precision<3>())
503  {
504  beta = Scalar(1)/Scalar(12) + t2/Scalar(720);
505  beta_dot_over_theta = Scalar(1)/Scalar(360);
506  }
507  else
508  {
509  const Scalar tinv = Scalar(1)/t,
510  t2inv = tinv*tinv;
511  Scalar st,ct; SINCOS (t, &st, &ct);
512  const Scalar inv_2_2ct = Scalar(1)/(Scalar(2)*(Scalar(1)-ct));
513 
514  beta = t2inv - st*tinv*inv_2_2ct;
515  beta_dot_over_theta = -Scalar(2)*t2inv*t2inv +
516  (Scalar(1) + st*tinv) * t2inv * inv_2_2ct;
517  }
518 
519  Scalar wTp = w.dot(p);
520 
521  Vector3 v3_tmp((beta_dot_over_theta*wTp)*w - (t2*beta_dot_over_theta+Scalar(2)*beta)*p);
522  // C can be treated as a temporary variable
523  C.noalias() = v3_tmp * w.transpose();
524  C.noalias() += beta * w * p.transpose();
525  C.diagonal().array() += wTp * beta;
526  addSkew(.5*p,C);
527 
528  B.noalias() = C * A;
529  C.setZero();
530  }
531 } // namespace pinocchio
532 
533 #include "pinocchio/spatial/explog-quaternion.hpp"
534 
535 #endif //#ifndef __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:79
void Jlog3(const Scalar &theta, const Eigen::MatrixBase< Vector3Like > &log, const Eigen::MatrixBase< Matrix3Like > &Jlog)
Derivative of log3.
Definition: explog.hpp:201
Eigen::Matrix< typename Vector3Like::Scalar, 3, 3, Vector3Like::Options > exp3(const Eigen::MatrixBase< Vector3Like > &v)
Exp: so3 -> SO3.
Definition: explog.hpp:32
void Jlog6(const SE3Tpl< Scalar, Options > &M, const Eigen::MatrixBase< Matrix6Like > &Jlog)
Derivative of log6 where and .
Definition: explog.hpp:473
MotionTpl< Scalar, Options > log6(const SE3Tpl< Scalar, Options > &M)
Log: SE3 -> se3.
Definition: explog.hpp:351
void SINCOS(const Scalar &a, Scalar *sa, Scalar *ca)
Computes sin/cos values of a given input scalar.
Definition: sincos.hpp:27
void Jexp3(const Eigen::MatrixBase< Vector3Like > &r, const Eigen::MatrixBase< Matrix3Like > &Jexp)
Derivative of .
Definition: explog.hpp:150
void Jexp6(const MotionDense< MotionDerived > &nu, const Eigen::MatrixBase< Matrix6Like > &Jexp)
Derivative of exp6 Computed as the inverse of Jlog6.
Definition: explog.hpp:401
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:60
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:274