src/spatial/explog.hpp
Go to the documentation of this file.
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 {
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  Scalar ct,st; SINCOS(t,&st,&ct);
46 
47  const Scalar alpha_vxvx = internal::if_then_else(internal::GT, t, TaylorSeriesExpansion<Scalar>::template precision<3>(),
48  (1 - ct)/t2,
49  Scalar(1)/Scalar(2) - t2/24);
50  const Scalar alpha_vx = internal::if_then_else(internal::GT, t, TaylorSeriesExpansion<Scalar>::template precision<3>(),
51  (st)/t,
52  Scalar(1) - t2/6);
53  Matrix3 res(alpha_vxvx * v * v.transpose());
54  res.coeffRef(0,1) -= alpha_vx * v[2]; res.coeffRef(1,0) += alpha_vx * v[2];
55  res.coeffRef(0,2) += alpha_vx * v[1]; res.coeffRef(2,0) -= alpha_vx * v[1];
56  res.coeffRef(1,2) -= alpha_vx * v[0]; res.coeffRef(2,1) += alpha_vx * v[0];
57 
58  ct = internal::if_then_else(internal::GT, t, TaylorSeriesExpansion<Scalar>::template precision<3>(),
59  ct,
60  Scalar(1) - t2/2);
61  res.diagonal().array() += ct;
62 
63  return res;
64  }
65 
73  template<typename Matrix3Like>
74  Eigen::Matrix<typename Matrix3Like::Scalar,3,1,PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix3Like)::Options>
75  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  Vector3 res;
82  log3_impl<Scalar>::run(R, theta, res);
83  return res;
84  }
85 
94  template<typename Matrix3Like>
95  Eigen::Matrix<typename Matrix3Like::Scalar,3,1,PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix3Like)::Options>
96  log3(const Eigen::MatrixBase<Matrix3Like> & R)
97  {
98  typename Matrix3Like::Scalar theta;
99  return log3(R.derived(),theta);
100  }
101 
110  template<AssignmentOperatorType op, typename Vector3Like, typename Matrix3Like>
111  void Jexp3(const Eigen::MatrixBase<Vector3Like> & r,
112  const Eigen::MatrixBase<Matrix3Like> & Jexp)
113  {
114  PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE (Vector3Like, r , 3, 1);
115  PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE (Matrix3Like, Jexp, 3, 3);
116 
117  Matrix3Like & Jout = PINOCCHIO_EIGEN_CONST_CAST(Matrix3Like,Jexp);
118  typedef typename Matrix3Like::Scalar Scalar;
119 
120  const Scalar n2 = r.squaredNorm();
121  const Scalar n = math::sqrt(n2);
122  const Scalar n_inv = Scalar(1)/n;
123  const Scalar n2_inv = n_inv * n_inv;
124  Scalar cn,sn; SINCOS(n,&sn,&cn);
125 
126  const Scalar a = internal::if_then_else(internal::LT, n, TaylorSeriesExpansion<Scalar>::template precision<3>(),
127  Scalar(1) - n2/Scalar(6),
128  sn*n_inv);
129  const Scalar b = internal::if_then_else(internal::LT, n, TaylorSeriesExpansion<Scalar>::template precision<3>(),
130  - Scalar(1)/Scalar(2) - n2/Scalar(24),
131  - (1-cn)*n2_inv);
132  const Scalar c = internal::if_then_else(internal::LT, n, TaylorSeriesExpansion<Scalar>::template precision<3>(),
133  Scalar(1)/Scalar(6) - n2/Scalar(120),
134  n2_inv * (1 - a));
135 
136  switch(op)
137  {
138  case SETTO:
139  Jout.diagonal().setConstant(a);
140  Jout(0,1) = -b*r[2]; Jout(1,0) = -Jout(0,1);
141  Jout(0,2) = b*r[1]; Jout(2,0) = -Jout(0,2);
142  Jout(1,2) = -b*r[0]; Jout(2,1) = -Jout(1,2);
143  Jout.noalias() += c * r * r.transpose();
144  break;
145  case ADDTO:
146  Jout.diagonal().array() += a;
147  Jout(0,1) += -b*r[2]; Jout(1,0) += b*r[2];
148  Jout(0,2) += b*r[1]; Jout(2,0) += -b*r[1];
149  Jout(1,2) += -b*r[0]; Jout(2,1) += b*r[0];
150  Jout.noalias() += c * r * r.transpose();
151  break;
152  case RMTO:
153  Jout.diagonal().array() -= a;
154  Jout(0,1) -= -b*r[2]; Jout(1,0) -= b*r[2];
155  Jout(0,2) -= b*r[1]; Jout(2,0) -= -b*r[1];
156  Jout(1,2) -= -b*r[0]; Jout(2,1) -= b*r[0];
157  Jout.noalias() -= c * r * r.transpose();
158  break;
159  default:
160  assert(false && "Wrong Op requesed value");
161  break;
162  }
163  }
164 
173  template<typename Vector3Like, typename Matrix3Like>
174  void Jexp3(const Eigen::MatrixBase<Vector3Like> & r,
175  const Eigen::MatrixBase<Matrix3Like> & Jexp)
176  {
177  Jexp3<SETTO>(r, Jexp);
178  }
179 
222  template<typename Scalar, typename Vector3Like, typename Matrix3Like>
223  void Jlog3(const Scalar & theta,
224  const Eigen::MatrixBase<Vector3Like> & log,
225  const Eigen::MatrixBase<Matrix3Like> & Jlog)
226  {
227  Jlog3_impl<Scalar>::run(theta, log,
228  PINOCCHIO_EIGEN_CONST_CAST(Matrix3Like,Jlog));
229  }
230 
243  template<typename Matrix3Like1, typename Matrix3Like2>
244  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  Vector3 w(log3(R,t));
252  Jlog3(t,w,PINOCCHIO_EIGEN_CONST_CAST(Matrix3Like2,Jlog));
253  }
254 
255  template<typename Scalar, typename Vector3Like1, typename Vector3Like2, typename Matrix3Like>
256  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  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  Scalar ctheta,stheta; SINCOS(theta,&stheta,&ctheta);
269 
270  Scalar denom = .5 / (1-ctheta),
271  a = theta * stheta * denom,
272  da_dt = (stheta - theta) * denom, // da / dtheta
273  b = ( 1 - a ) / (theta*theta),
274  //db_dt = - (2 * (1 - a) / theta + da_dt ) / theta**2; // db / dtheta
275  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  Vector3 dl_dv_v (a*v + .5*log.cross(v) + b*log*log.transpose()*v);
281 
282  Scalar dt_dv_v = log.dot(dl_dv_v) / theta;
283 
284  // Derivative of b * log * log^T
285  vt_Hlog_.noalias() = db_dt * dt_dv_v * log * log.transpose();
286  vt_Hlog_.noalias() += b * dl_dv_v * log.transpose();
287  vt_Hlog_.noalias() += b * log * dl_dv_v.transpose();
288  // Derivative of .5 * [ log ]
289  addSkew(.5 * dl_dv_v, vt_Hlog_);
290  // Derivative of a * I3
291  vt_Hlog_.diagonal().array() += da_dt * dt_dv_v;
292  }
293 
302  template<typename Matrix3Like1, typename Vector3Like, typename Matrix3Like2>
303  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  Vector3 w(log3(R,t));
312  Hlog3(t,w,v,PINOCCHIO_EIGEN_CONST_CAST(Matrix3Like2,vt_Hlog));
313  }
314 
324  template<typename MotionDerived>
325  SE3Tpl<typename MotionDerived::Scalar,PINOCCHIO_EIGEN_PLAIN_TYPE(typename MotionDerived::Vector3)::Options>
326  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  SE3 res;
334  typename SE3::LinearType & trans = res.translation();
335  typename SE3::AngularType & rot = res.rotation();
336 
337  const typename MotionDerived::ConstAngularType & w = nu.angular();
338  const typename MotionDerived::ConstLinearType & v = nu.linear();
339 
340  Scalar alpha_wxv, alpha_v, alpha_w, diagonal_term;
341  const Scalar t2 = w.squaredNorm();
342  const Scalar t = math::sqrt(t2);
343  Scalar ct,st; SINCOS(t,&st,&ct);
344  const Scalar inv_t2 = Scalar(1)/t2;
345 
346  alpha_wxv = internal::if_then_else(internal::LT, t, TaylorSeriesExpansion<Scalar>::template precision<3>(),
347  Scalar(1)/Scalar(2) - t2/24,
348  (Scalar(1) - ct)*inv_t2);
349 
350  alpha_v = internal::if_then_else(internal::LT, t, TaylorSeriesExpansion<Scalar>::template precision<3>(),
351  Scalar(1) - t2/6,
352  (st)/t);
353 
354  alpha_w = internal::if_then_else(internal::LT, t, TaylorSeriesExpansion<Scalar>::template precision<3>(),
355  (Scalar(1)/Scalar(6) - t2/120),
356  (Scalar(1) - alpha_v)*inv_t2);
357 
358  diagonal_term = internal::if_then_else(internal::LT, t, TaylorSeriesExpansion<Scalar>::template precision<3>(),
359  Scalar(1) - t2/2,
360  ct);
361 
362  // Linear
363  trans.noalias() = (alpha_v*v + (alpha_w*w.dot(v))*w + alpha_wxv*w.cross(v));
364 
365  // Rotational
366  rot.noalias() = alpha_wxv * w * w.transpose();
367  rot.coeffRef(0,1) -= alpha_v * w[2]; rot.coeffRef(1,0) += alpha_v * w[2];
368  rot.coeffRef(0,2) += alpha_v * w[1]; rot.coeffRef(2,0) -= alpha_v * w[1];
369  rot.coeffRef(1,2) -= alpha_v * w[0]; rot.coeffRef(2,1) += alpha_v * w[0];
370  rot.diagonal().array() += diagonal_term;
371 
372  return res;
373  }
374 
383  template<typename Vector6Like>
384  SE3Tpl<typename Vector6Like::Scalar,PINOCCHIO_EIGEN_PLAIN_TYPE(Vector6Like)::Options>
385  exp6(const Eigen::MatrixBase<Vector6Like> & v)
386  {
387  PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE (Vector6Like, v, 6, 1);
388 
389  MotionRef<const Vector6Like> nu(v.derived());
390  return exp6(nu);
391  }
392 
401  template<typename Scalar, int Options>
402  MotionTpl<Scalar,Options>
403  log6(const SE3Tpl<Scalar,Options> & M)
404  {
405  typedef MotionTpl<Scalar,Options> Motion;
406  Motion mout;
407  log6_impl<Scalar>::run(M, mout);
408  return mout;
409  }
410 
419  template<typename Matrix4Like>
420  MotionTpl<typename Matrix4Like::Scalar,Eigen::internal::traits<Matrix4Like>::Options>
421  log6(const Eigen::MatrixBase<Matrix4Like> & M)
422  {
423  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  SE3 m(M);
431  Motion mout;
432  log6_impl<Scalar>::run(m, mout);
433  return mout;
434  }
435 
438  template<AssignmentOperatorType op, typename MotionDerived, typename Matrix6Like>
439  void Jexp6(const MotionDense<MotionDerived> & nu,
440  const Eigen::MatrixBase<Matrix6Like> & Jexp)
441  {
442  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  Matrix6Like & Jout = PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like,Jexp);
448 
449  const typename MotionDerived::ConstLinearType & v = nu.linear();
450  const typename MotionDerived::ConstAngularType & w = nu.angular();
451  const Scalar t2 = w.squaredNorm();
452  const Scalar t = math::sqrt(t2);
453 
454  const Scalar tinv = Scalar(1)/t,
455  t2inv = tinv*tinv;
456  Scalar st,ct; SINCOS (t, &st, &ct);
457  const Scalar inv_2_2ct = Scalar(1)/(Scalar(2)*(Scalar(1)-ct));
458 
459 
460  const Scalar beta = internal::if_then_else(internal::LT, t, TaylorSeriesExpansion<Scalar>::template precision<3>(),
461  Scalar(1)/Scalar(12) + t2/Scalar(720),
462  t2inv - st*tinv*inv_2_2ct);
463 
464  const Scalar beta_dot_over_theta = internal::if_then_else(internal::LT, t, TaylorSeriesExpansion<Scalar>::template precision<3>(),
465  Scalar(1)/Scalar(360),
466  -Scalar(2)*t2inv*t2inv + (Scalar(1) + st*tinv) * t2inv * inv_2_2ct);
467 
468  switch(op)
469  {
470  case SETTO:
471  {
472  Jexp3<SETTO>(w, Jout.template bottomRightCorner<3,3>());
473  Jout.template topLeftCorner<3,3>() = Jout.template bottomRightCorner<3,3>();
474  const Vector3 p = Jout.template topLeftCorner<3,3>().transpose() * v;
475  const Scalar wTp (w.dot (p));
476  const Matrix3 J (alphaSkew(.5, p) +
477  (beta_dot_over_theta*wTp) *w*w.transpose()
478  - (t2*beta_dot_over_theta+Scalar(2)*beta)*p*w.transpose()
479  + wTp * beta * Matrix3::Identity()
480  + beta *w*p.transpose());
481  Jout.template topRightCorner<3,3>().noalias() =
482  - Jout.template topLeftCorner<3,3>() * J;
483  Jout.template bottomLeftCorner<3,3>().setZero();
484  break;
485  }
486  case ADDTO:
487  {
488  Matrix3 Jtmp3;
489  Jexp3<SETTO>(w, Jtmp3);
490  Jout.template bottomRightCorner<3,3>() += Jtmp3;
491  Jout.template topLeftCorner<3,3>() += Jtmp3;
492  const Vector3 p = Jtmp3.transpose() * v;
493  const Scalar wTp (w.dot (p));
494  const Matrix3 J (alphaSkew(.5, p) +
495  (beta_dot_over_theta*wTp) *w*w.transpose()
496  - (t2*beta_dot_over_theta+Scalar(2)*beta)*p*w.transpose()
497  + wTp * beta * Matrix3::Identity()
498  + beta *w*p.transpose());
499  Jout.template topRightCorner<3,3>().noalias() +=
500  - Jtmp3 * J;
501  break;
502  }
503  case RMTO:
504  {
505  Matrix3 Jtmp3;
506  Jexp3<SETTO>(w, Jtmp3);
507  Jout.template bottomRightCorner<3,3>() -= Jtmp3;
508  Jout.template topLeftCorner<3,3>() -= Jtmp3;
509  const Vector3 p = Jtmp3.transpose() * v;
510  const Scalar wTp (w.dot (p));
511  const Matrix3 J (alphaSkew(.5, p) +
512  (beta_dot_over_theta*wTp) *w*w.transpose()
513  - (t2*beta_dot_over_theta+Scalar(2)*beta)*p*w.transpose()
514  + wTp * beta * Matrix3::Identity()
515  + beta *w*p.transpose());
516  Jout.template topRightCorner<3,3>().noalias() -=
517  - Jtmp3 * J;
518  break;
519  }
520  default:
521  assert(false && "Wrong Op requesed value");
522  break;
523  }
524  }
525 
528  template<typename MotionDerived, typename Matrix6Like>
529  void Jexp6(const MotionDense<MotionDerived> & nu,
530  const Eigen::MatrixBase<Matrix6Like> & Jexp)
531  {
532  Jexp6<SETTO>(nu, Jexp);
533  }
534 
593  template<typename Scalar, int Options, typename Matrix6Like>
594  void Jlog6(const SE3Tpl<Scalar, Options> & M,
595  const Eigen::MatrixBase<Matrix6Like> & Jlog)
596  {
597  Jlog6_impl<Scalar>::run(M,PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like,Jlog));
598  }
599 
600  template<typename Scalar, int Options>
601  template<typename OtherScalar>
602  SE3Tpl<Scalar,Options> SE3Tpl<Scalar,Options>::Interpolate(const SE3Tpl & A,
603  const SE3Tpl & B,
604  const OtherScalar & alpha)
605  {
606  typedef SE3Tpl<Scalar,Options> ReturnType;
607  typedef MotionTpl<Scalar,Options> Motion;
608 
609  Motion dv = log6(A.actInv(B));
610  ReturnType res = A * exp6(alpha*dv);
611  return res;
612  }
613 
614 } // namespace pinocchio
615 
616 #include "pinocchio/spatial/explog-quaternion.hpp"
617 #include "pinocchio/spatial/log.hxx"
618 
619 #endif //#ifndef __pinocchio_spatial_explog_hpp__
pinocchio::log3
Eigen::Matrix< typename Matrix3Like::Scalar, 3, 1, PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix3Like)::Options > log3(const Eigen::MatrixBase< Matrix3Like > &R, typename Matrix3Like::Scalar &theta)
Same as log3.
Definition: src/spatial/explog.hpp:75
pinocchio::v
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > const Eigen::MatrixBase< TangentVectorType > & v
Definition: joint-configuration.hpp:746
pinocchio::Jexp3
void Jexp3(const Eigen::MatrixBase< Vector3Like > &r, const Eigen::MatrixBase< Matrix3Like > &Jexp)
Derivative of .
Definition: src/spatial/explog.hpp:111
PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE
#define PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE(type, M, nrows, ncols)
Ensure that a matrix (or vector) is of correct size (compile-time and run-time assertion) ...
Definition: src/macros.hpp:54
n
Vec3f n
b
Vec3f b
pinocchio::SE3Base::actInv
SE3GroupAction< D >::ReturnType actInv(const D &d) const
by = aXb.actInv(ay)
Definition: se3-base.hpp:97
pinocchio::Jlog3
void Jlog3(const Scalar &theta, const Eigen::MatrixBase< Vector3Like > &log, const Eigen::MatrixBase< Matrix3Like > &Jlog)
Derivative of log3.
Definition: src/spatial/explog.hpp:223
r
FCL_REAL r
pinocchio::Jlog6
void Jlog6(const SE3Tpl< Scalar, Options > &M, const Eigen::MatrixBase< Matrix6Like > &Jlog)
Derivative of log6.
Definition: src/spatial/explog.hpp:594
pinocchio::log6
MotionTpl< Scalar, Options > log6(const SE3Tpl< Scalar, Options > &M)
Log: SE3 -> se3.
Definition: src/spatial/explog.hpp:403
pinocchio::log6
MotionTpl< typename Matrix4Like::Scalar, Eigen::internal::traits< Matrix4Like >::Options > log6(const Eigen::MatrixBase< Matrix4Like > &M)
Log: SE3 -> se3.
Definition: src/spatial/explog.hpp:421
pinocchio::exp6
SE3Tpl< typename Vector6Like::Scalar, PINOCCHIO_EIGEN_PLAIN_TYPE(Vector6Like)::Options > exp6(const Eigen::MatrixBase< Vector6Like > &v)
Exp: se3 -> SE3.
Definition: src/spatial/explog.hpp:385
c
Vec3f c
pinocchio::Jexp6
void Jexp6(const MotionDense< MotionDerived > &nu, const Eigen::MatrixBase< Matrix6Like > &Jexp)
Derivative of exp6 Computed as the inverse of Jlog6.
Definition: src/spatial/explog.hpp:439
pinocchio::MotionBase::linear
ConstLinearType linear() const
Definition: motion-base.hpp:22
PINOCCHIO_EIGEN_CONST_CAST
#define PINOCCHIO_EIGEN_CONST_CAST(TYPE, OBJ)
Macro for an automatic const_cast.
Definition: eigen-macros.hpp:43
pinocchio::MotionBase::angular
ConstAngularType angular() const
Definition: motion-base.hpp:21
pinocchio::python::Scalar
SE3::Scalar Scalar
Definition: conversions.cpp:13
pinocchio::exp3
Eigen::Matrix< typename Vector3Like::Scalar, 3, 3, PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like)::Options > exp3(const Eigen::MatrixBase< Vector3Like > &v)
Exp: so3 -> SO3.
Definition: src/spatial/explog.hpp:34
pinocchio::Motion
MotionTpl< double, 0 > Motion
Definition: src/spatial/fwd.hpp:57
a
Vec3f a
pinocchio::SINCOS
void SINCOS(const S1 &a, S2 *sa, S3 *ca)
Computes sin/cos values of a given input scalar.
Definition: sincos.hpp:26
m
float m
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:60
pinocchio::SE3Base::rotation
ConstAngularRef rotation() const
Definition: se3-base.hpp:37
pinocchio
Main pinocchio namespace.
Definition: timings.cpp:28
pinocchio::PINOCCHIO_EIGEN_PLAIN_TYPE
PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorType) integrate(const ModelTpl< Scalar
Integrate a configuration vector for the specified model for a tangent vector during one unit time...
res
res
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. i.e. the antisymmetric matrix representation of the cross product operator ( )
Definition: skew.hpp:124
omniidl_be_python_with_docstring.run
def run(tree, args)
Definition: cmake/hpp/idl/omniidl_be_python_with_docstring.py:140
setZero
void setZero(std::vector< MatType, Eigen::aligned_allocator< MatType > > &Ms)
pinocchio::Hlog3
void Hlog3(const Scalar &theta, const Eigen::MatrixBase< Vector3Like1 > &log, const Eigen::MatrixBase< Vector3Like2 > &v, const Eigen::MatrixBase< Matrix3Like > &vt_Hlog)
Definition: src/spatial/explog.hpp:256
pinocchio::internal::if_then_else
if_then_else_impl< LhsType, RhsType, ThenType, ElseType >::ReturnType if_then_else(const ComparisonOperators op, const LhsType &lhs_value, const RhsType &rhs_value, const ThenType &then_value, const ElseType &else_value)
Definition: utils/static-if.hpp:77
t
Transform3f t
pinocchio::exp6
SE3Tpl< typename MotionDerived::Scalar, PINOCCHIO_EIGEN_PLAIN_TYPE(typename MotionDerived::Vector3)::Options > exp6(const MotionDense< MotionDerived > &nu)
Exp: se3 -> SE3.
Definition: src/spatial/explog.hpp:326
ur5x4.w
w
Definition: ur5x4.py:45
pinocchio::SE3
SE3Tpl< double, 0 > SE3
Definition: src/spatial/fwd.hpp:53
pinocchio::SE3Tpl::Interpolate
static SE3Tpl Interpolate(const SE3Tpl &A, const SE3Tpl &B, const OtherScalar &alpha)
Linear interpolation on the SE3 manifold.
pinocchio::SE3Base::translation
ConstLinearRef translation() const
Definition: se3-base.hpp:38

pinocchio
Author(s):
autogenerated on Fri Jun 23 2023 02:38:29