6 #ifndef __pinocchio_spatial_explog_hpp__
7 #define __pinocchio_spatial_explog_hpp__
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"
18 #include <Eigen/Geometry>
20 #include "pinocchio/spatial/log.hpp"
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)
36 PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE (Vector3Like, v, 3, 1);
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;
42 const Scalar t2 = v.squaredNorm();
44 const Scalar t = math::sqrt(t2);
45 Scalar ct,st;
SINCOS(t,&st,&ct);
49 Scalar(1)/Scalar(2) - t2/24);
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];
61 res.diagonal().array() += ct;
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)
78 typedef typename Matrix3Like::Scalar Scalar;
79 typedef Eigen::Matrix<Scalar,3,1,
80 PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix3Like)::Options> Vector3;
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)
98 typename Matrix3Like::Scalar theta;
99 return log3(R.derived(),theta);
110 template<AssignmentOperatorType op,
typename Vector3Like,
typename Matrix3Like>
111 void Jexp3(
const Eigen::MatrixBase<Vector3Like> & r,
112 const Eigen::MatrixBase<Matrix3Like> & Jexp)
114 PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE (Vector3Like, r , 3, 1);
115 PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE (Matrix3Like, Jexp, 3, 3);
117 Matrix3Like & Jout = PINOCCHIO_EIGEN_CONST_CAST(Matrix3Like,Jexp);
118 typedef typename Matrix3Like::Scalar Scalar;
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);
127 Scalar(1) - n2/Scalar(6),
130 - Scalar(1)/Scalar(2) - n2/Scalar(24),
133 Scalar(1)/Scalar(6) - n2/Scalar(120),
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();
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();
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();
160 assert(
false &&
"Wrong Op requesed value");
173 template<
typename Vector3Like,
typename Matrix3Like>
174 void Jexp3(
const Eigen::MatrixBase<Vector3Like> & r,
175 const Eigen::MatrixBase<Matrix3Like> & Jexp)
177 Jexp3<SETTO>(r, Jexp);
192 template<
typename Scalar,
typename Vector3Like,
typename Matrix3Like>
194 const Eigen::MatrixBase<Vector3Like> & log,
195 const Eigen::MatrixBase<Matrix3Like> & Jlog)
198 PINOCCHIO_EIGEN_CONST_CAST(Matrix3Like,Jlog));
213 template<
typename Matrix3Like1,
typename Matrix3Like2>
214 void Jlog3(
const Eigen::MatrixBase<Matrix3Like1> & R,
215 const Eigen::MatrixBase<Matrix3Like2> & Jlog)
217 typedef typename Matrix3Like1::Scalar Scalar;
218 typedef Eigen::Matrix<Scalar,3,1,PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix3Like1)::Options> Vector3;
221 Vector3 w(
log3(R,t));
222 Jlog3(t,w,PINOCCHIO_EIGEN_CONST_CAST(Matrix3Like2,Jlog));
225 template<
typename Scalar,
typename Vector3Like1,
typename Vector3Like2,
typename Matrix3Like>
226 void Hlog3(
const Scalar & theta,
227 const Eigen::MatrixBase<Vector3Like1> & log,
228 const Eigen::MatrixBase<Vector3Like2> & v,
229 const Eigen::MatrixBase<Matrix3Like> & vt_Hlog)
231 typedef Eigen::Matrix<Scalar,3,1,PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix3Like)::Options> Vector3;
232 Matrix3Like & vt_Hlog_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix3Like,vt_Hlog);
238 Scalar ctheta,stheta;
SINCOS(theta,&stheta,&ctheta);
240 Scalar denom = .5 / (1-ctheta),
241 a = theta * stheta * denom,
242 da_dt = (stheta - theta) * denom,
243 b = ( 1 - a ) / (theta*theta),
245 db_dt = - (2 / theta - (theta + stheta) * denom) / (theta*theta);
250 Vector3 dl_dv_v (a*v + .5*log.cross(v) + b*log*log.transpose()*v);
252 Scalar dt_dv_v = log.dot(dl_dv_v) / theta;
255 vt_Hlog_.noalias() = db_dt * dt_dv_v * log * log.transpose();
256 vt_Hlog_.noalias() += b * dl_dv_v * log.transpose();
257 vt_Hlog_.noalias() += b * log * dl_dv_v.transpose();
259 addSkew(.5 * dl_dv_v, vt_Hlog_);
261 vt_Hlog_.diagonal().array() += da_dt * dt_dv_v;
272 template<
typename Matrix3Like1,
typename Vector3Like,
typename Matrix3Like2>
273 void Hlog3(
const Eigen::MatrixBase<Matrix3Like1> & R,
274 const Eigen::MatrixBase<Vector3Like> & v,
275 const Eigen::MatrixBase<Matrix3Like2> & vt_Hlog)
277 typedef typename Matrix3Like1::Scalar Scalar;
278 typedef Eigen::Matrix<Scalar,3,1,PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix3Like1)::Options> Vector3;
281 Vector3 w(
log3(R,t));
282 Hlog3(t,w,v,PINOCCHIO_EIGEN_CONST_CAST(Matrix3Like2,vt_Hlog));
294 template<
typename MotionDerived>
295 SE3Tpl<
typename MotionDerived::Scalar,PINOCCHIO_EIGEN_PLAIN_TYPE(
typename MotionDerived::Vector3)::Options>
298 typedef typename MotionDerived::Scalar Scalar;
299 enum { Options = PINOCCHIO_EIGEN_PLAIN_TYPE(
typename MotionDerived::Vector3)::Options };
304 typename SE3::LinearType & trans = res.translation();
305 typename SE3::AngularType & rot = res.rotation();
307 const typename MotionDerived::ConstAngularType & w = nu.angular();
308 const typename MotionDerived::ConstLinearType & v = nu.linear();
310 Scalar alpha_wxv, alpha_v, alpha_w, diagonal_term;
311 const Scalar t2 = w.squaredNorm();
312 const Scalar t = math::sqrt(t2);
313 Scalar ct,st;
SINCOS(t,&st,&ct);
314 const Scalar inv_t2 = Scalar(1)/t2;
317 Scalar(1)/Scalar(2) - t2/24,
318 (Scalar(1) - ct)*inv_t2);
325 (Scalar(1)/Scalar(6) - t2/120),
326 (Scalar(1) - alpha_v)*inv_t2);
333 trans.noalias() = (alpha_v*v + (alpha_w*w.dot(v))*w + alpha_wxv*w.cross(v));
336 rot.noalias() = alpha_wxv * w * w.transpose();
337 rot.coeffRef(0,1) -= alpha_v * w[2]; rot.coeffRef(1,0) += alpha_v * w[2];
338 rot.coeffRef(0,2) += alpha_v * w[1]; rot.coeffRef(2,0) -= alpha_v * w[1];
339 rot.coeffRef(1,2) -= alpha_v * w[0]; rot.coeffRef(2,1) += alpha_v * w[0];
340 rot.diagonal().array() += diagonal_term;
353 template<
typename Vector6Like>
354 SE3Tpl<
typename Vector6Like::Scalar,PINOCCHIO_EIGEN_PLAIN_TYPE(Vector6Like)::Options>
355 exp6(const Eigen::MatrixBase<Vector6Like> & v)
357 PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE (Vector6Like, v, 6, 1);
371 template<
typename Scalar,
int Options>
372 MotionTpl<Scalar,Options>
389 template<
typename Matrix4Like>
390 MotionTpl<typename Matrix4Like::Scalar,Eigen::internal::traits<Matrix4Like>::Options>
391 log6(
const Eigen::MatrixBase<Matrix4Like> & M)
393 PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix4Like, M, 4, 4);
395 typedef typename Matrix4Like::Scalar Scalar;
396 enum {Options = Eigen::internal::traits<Matrix4Like>::Options};
408 template<AssignmentOperatorType op,
typename MotionDerived,
typename Matrix6Like>
410 const Eigen::MatrixBase<Matrix6Like> & Jexp)
412 PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE (Matrix6Like, Jexp, 6, 6);
414 typedef typename MotionDerived::Scalar Scalar;
415 typedef typename MotionDerived::Vector3 Vector3;
416 typedef Eigen::Matrix<Scalar, 3, 3, Vector3::Options> Matrix3;
417 Matrix6Like & Jout = PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like,Jexp);
419 const typename MotionDerived::ConstLinearType & v = nu.linear();
420 const typename MotionDerived::ConstAngularType & w = nu.angular();
421 const Scalar t2 = w.squaredNorm();
422 const Scalar t = math::sqrt(t2);
424 const Scalar tinv = Scalar(1)/t,
426 Scalar st,ct;
SINCOS (t, &st, &ct);
427 const Scalar inv_2_2ct = Scalar(1)/(Scalar(2)*(Scalar(1)-ct));
431 Scalar(1)/Scalar(12) + t2/Scalar(720),
432 t2inv - st*tinv*inv_2_2ct);
435 Scalar(1)/Scalar(360),
436 -Scalar(2)*t2inv*t2inv + (Scalar(1) + st*tinv) * t2inv * inv_2_2ct);
442 Jexp3<SETTO>(w, Jout.template bottomRightCorner<3,3>());
443 Jout.template topLeftCorner<3,3>() = Jout.template bottomRightCorner<3,3>();
444 const Vector3 p = Jout.template topLeftCorner<3,3>().transpose() * v;
445 const Scalar wTp (w.dot (p));
447 (beta_dot_over_theta*wTp) *w*w.transpose()
448 - (t2*beta_dot_over_theta+Scalar(2)*beta)*p*w.transpose()
449 + wTp * beta * Matrix3::Identity()
450 + beta *w*p.transpose());
451 Jout.template topRightCorner<3,3>().noalias() =
452 - Jout.template topLeftCorner<3,3>() * J;
453 Jout.template bottomLeftCorner<3,3>().setZero();
459 Jexp3<SETTO>(w, Jtmp3);
460 Jout.template bottomRightCorner<3,3>() += Jtmp3;
461 Jout.template topLeftCorner<3,3>() += Jtmp3;
462 const Vector3 p = Jtmp3.transpose() * v;
463 const Scalar wTp (w.dot (p));
465 (beta_dot_over_theta*wTp) *w*w.transpose()
466 - (t2*beta_dot_over_theta+Scalar(2)*beta)*p*w.transpose()
467 + wTp * beta * Matrix3::Identity()
468 + beta *w*p.transpose());
469 Jout.template topRightCorner<3,3>().noalias() +=
476 Jexp3<SETTO>(w, Jtmp3);
477 Jout.template bottomRightCorner<3,3>() -= Jtmp3;
478 Jout.template topLeftCorner<3,3>() -= Jtmp3;
479 const Vector3 p = Jtmp3.transpose() * v;
480 const Scalar wTp (w.dot (p));
482 (beta_dot_over_theta*wTp) *w*w.transpose()
483 - (t2*beta_dot_over_theta+Scalar(2)*beta)*p*w.transpose()
484 + wTp * beta * Matrix3::Identity()
485 + beta *w*p.transpose());
486 Jout.template topRightCorner<3,3>().noalias() -=
491 assert(
false &&
"Wrong Op requesed value");
498 template<
typename MotionDerived,
typename Matrix6Like>
500 const Eigen::MatrixBase<Matrix6Like> & Jexp)
502 Jexp6<SETTO>(nu, Jexp);
537 template<
typename Scalar,
int Options,
typename Matrix6Like>
539 const Eigen::MatrixBase<Matrix6Like> & Jlog)
544 template<
typename Scalar,
int Options>
545 template<
typename OtherScalar>
548 const OtherScalar & alpha)
550 typedef SE3Tpl<Scalar,Options> ReturnType;
551 typedef MotionTpl<Scalar,Options> Motion;
553 Motion dv = log6(A.actInv(B));
554 ReturnType res = A * exp6(alpha*dv);
560 #include "pinocchio/spatial/explog-quaternion.hpp"
561 #include "pinocchio/spatial/log.hxx"
563 #endif //#ifndef __pinocchio_spatial_explog_hpp__