6 #ifndef __spatial_explog_hpp__ 7 #define __spatial_explog_hpp__ 9 #include <Eigen/Geometry> 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" 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)
34 PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE (Vector3Like, v, 3, 1);
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;
40 const Scalar t2 = v.squaredNorm();
42 const Scalar t = math::sqrt(t2);
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;
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;
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)
82 PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix3Like, R, 3, 3);
84 typedef typename Matrix3Like::Scalar Scalar;
85 typedef Eigen::Matrix<Scalar,3,1,PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix3Like)::Options> Vector3;
87 static const Scalar PI_value = PI<Scalar>();
90 const Scalar tr = R.trace();
91 if(tr > Scalar(3)) theta = 0;
92 else if(tr < Scalar(-1)) theta = PI_value;
93 else theta = math::acos((tr - Scalar(1))/Scalar(2));
94 assert(theta == theta &&
"theta contains some NaN");
97 if(theta >= PI_value - 1e-2)
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)); }
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));
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)
135 PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE (Matrix3Like, R, 3, 3);
137 typename Matrix3Like::Scalar theta;
138 return log3(R.derived(),theta);
149 template<
typename Vector3Like,
typename Matrix3Like>
150 void Jexp3(
const Eigen::MatrixBase<Vector3Like> & r,
151 const Eigen::MatrixBase<Matrix3Like> & Jexp)
153 PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE (Vector3Like, r , 3, 1);
154 PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE (Matrix3Like, Jexp, 3, 3);
156 Matrix3Like & Jout = PINOCCHIO_EIGEN_CONST_CAST(Matrix3Like,Jexp);
157 typedef typename Matrix3Like::Scalar Scalar;
159 Scalar n2 = r.squaredNorm(),a,b,c;
160 Scalar n = math::sqrt(n2);
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);
170 Scalar n_inv = Scalar(1)/n;
171 Scalar n2_inv = n_inv * n_inv;
172 Scalar cn,sn;
SINCOS(n,&sn,&cn);
176 c = n2_inv * (1 - a);
179 Jout.diagonal().setConstant(a);
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);
185 Jout.noalias() += c * r * r.transpose();
200 template<
typename Scalar,
typename Vector3Like,
typename Matrix3Like>
202 const Eigen::MatrixBase<Vector3Like> & log,
203 const Eigen::MatrixBase<Matrix3Like> & Jlog)
205 PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE (Vector3Like, log, 3, 1);
206 PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE (Matrix3Like, Jlog, 3, 3);
208 Matrix3Like & Jout = PINOCCHIO_EIGEN_CONST_CAST(Matrix3Like,Jlog);
212 const Scalar alpha = Scalar(1)/Scalar(12) + theta*theta / Scalar(720);
213 Jout.noalias() = alpha * log * log.transpose();
215 Jout.diagonal().array() += Scalar(0.5) * (2 - theta*theta / Scalar(6));
223 Scalar ct,st;
SINCOS(theta,&st,&ct);
224 const Scalar st_1mct = st/(Scalar(1)-ct);
226 const Scalar alpha = Scalar(1)/(theta*theta) - st_1mct/(Scalar(2)*theta);
227 Jout.noalias() = alpha * log * log.transpose();
229 Jout.diagonal().array() += Scalar(0.5) * (theta*st_1mct);
248 template<
typename Matrix3Like1,
typename Matrix3Like2>
249 void Jlog3(
const Eigen::MatrixBase<Matrix3Like1> & R,
250 const Eigen::MatrixBase<Matrix3Like2> & Jlog)
252 PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE (Matrix3Like1, R, 3, 3);
253 PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE (Matrix3Like2, Jlog, 3, 3);
255 typedef typename Matrix3Like1::Scalar Scalar;
256 typedef Eigen::Matrix<Scalar,3,1,PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix3Like1)::Options> Vector3;
259 Vector3 w(
log3(R,t));
260 Jlog3(t,w,PINOCCHIO_EIGEN_CONST_CAST(Matrix3Like2,Jlog));
272 template<
typename MotionDerived>
276 typedef typename MotionDerived::Scalar Scalar;
277 enum { Options = PINOCCHIO_EIGEN_PLAIN_TYPE(
typename MotionDerived::Vector3)::Options };
282 typename SE3::LinearType & trans = res.translation();
283 typename SE3::AngularType & rot = res.rotation();
285 const typename MotionDerived::ConstAngularType & w = nu.angular();
286 const typename MotionDerived::ConstLinearType & v = nu.linear();
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;
295 Scalar(1)/Scalar(2) - t2/24,
296 (Scalar(1) - ct)*inv_t2);
303 (Scalar(1)/Scalar(6) - t2/120),
304 (Scalar(1) - alpha_v)*inv_t2);
311 trans.noalias() = (alpha_v*v + (alpha_w*w.dot(v))*w + alpha_wxv*w.cross(v));
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;
331 template<
typename Vector6Like>
333 exp6(
const Eigen::MatrixBase<Vector6Like> & v)
335 PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE (Vector6Like, v, 6, 1);
349 template <
typename Scalar,
int Options>
355 typedef typename SE3::Vector3 Vector3;
357 typename SE3::ConstAngularRef R = M.rotation();
358 typename SE3::ConstLinearRef p = M.translation();
361 Vector3 w(
log3(R,t));
362 const Scalar t2 = t*t;
366 alpha = Scalar(1) - t2/Scalar(12) - t2*t2/Scalar(720);
367 beta = Scalar(1)/Scalar(12) + t2/Scalar(720);
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));
376 return Motion(alpha * p - 0.5 * w.cross(p) + beta * w.dot(p) * w,
388 template<
typename Matrix4Like>
390 log6(
const Eigen::MatrixBase<Matrix4Like> & M)
392 PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE (Matrix4Like, M, 4, 4);
400 template<
typename MotionDerived,
typename Matrix6Like>
402 const Eigen::MatrixBase<Matrix6Like> & Jexp)
404 PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE (Matrix6Like, Jexp, 6, 6);
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);
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);
418 Jexp3(w, Jout.template bottomRightCorner<3,3>());
419 Jout.template topLeftCorner<3,3>() = Jout.template bottomRightCorner<3,3>();
421 Scalar beta, beta_dot_over_theta;
424 beta = Scalar(1)/Scalar(12) + t2/Scalar(720);
425 beta_dot_over_theta = Scalar(1)/Scalar(360);
429 const Scalar tinv = Scalar(1)/t,
431 Scalar st,ct;
SINCOS (t, &st, &ct);
432 const Scalar inv_2_2ct = Scalar(1)/(Scalar(2)*(Scalar(1)-ct));
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;
439 Vector3 p (Jout.template topLeftCorner<3,3>().transpose() * v);
440 Scalar wTp (w.dot (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());
447 Jout.template topRightCorner<3,3>().noalias() =
448 - Jout.template topLeftCorner<3,3>() * J;
449 Jout.template bottomLeftCorner<3,3>().setZero();
472 template<
typename Scalar,
int Options,
typename Matrix6Like>
474 const Eigen::MatrixBase<Matrix6Like> & Jlog)
476 PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE (Matrix6Like, Jlog, 6, 6);
479 typedef typename SE3::Vector3 Vector3;
480 Matrix6Like & value = PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like,Jlog);
482 typename SE3::ConstAngularRef R = M.rotation();
483 typename SE3::ConstLinearRef p = M.translation();
486 Vector3 w(
log3(R,t));
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>();
500 const Scalar t2 = t*t;
501 Scalar beta, beta_dot_over_theta;
504 beta = Scalar(1)/Scalar(12) + t2/Scalar(720);
505 beta_dot_over_theta = Scalar(1)/Scalar(360);
509 const Scalar tinv = Scalar(1)/t,
511 Scalar st,ct;
SINCOS (t, &st, &ct);
512 const Scalar inv_2_2ct = Scalar(1)/(Scalar(2)*(Scalar(1)-ct));
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;
519 Scalar wTp = w.dot(p);
521 Vector3 v3_tmp((beta_dot_over_theta*wTp)*w - (t2*beta_dot_over_theta+Scalar(2)*beta)*p);
523 C.noalias() = v3_tmp * w.transpose();
524 C.noalias() += beta * w * p.transpose();
525 C.diagonal().array() += wTp * beta;
532 template<
typename Scalar,
int Options>
533 template<
typename OtherScalar>
536 const OtherScalar & alpha)
542 ReturnType res = A *
exp6(alpha*dv);
548 #include "pinocchio/spatial/explog-quaternion.hpp" 550 #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.
void Jlog3(const Scalar &theta, const Eigen::MatrixBase< Vector3Like > &log, const Eigen::MatrixBase< Matrix3Like > &Jlog)
Derivative of log3.
Eigen::Matrix< typename Vector3Like::Scalar, 3, 3, Vector3Like ::Options > exp3(const Eigen::MatrixBase< Vector3Like > &v)
Exp: so3 -> SO3.
void Jlog6(const SE3Tpl< Scalar, Options > &M, const Eigen::MatrixBase< Matrix6Like > &Jlog)
Derivative of log6 where and .
MotionTpl< Scalar, Options > log6(const SE3Tpl< Scalar, Options > &M)
Log: SE3 -> se3.
void Jexp3(const Eigen::MatrixBase< Vector3Like > &r, const Eigen::MatrixBase< Matrix3Like > &Jexp)
Derivative of .
void Jexp6(const MotionDense< MotionDerived > &nu, const Eigen::MatrixBase< Matrix6Like > &Jexp)
Derivative of exp6 Computed as the inverse of Jlog6.
void SINCOS(const S1 &a, S2 *sa, S3 *ca)
Computes sin/cos values of a given input scalar.
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...
Main pinocchio namespace.
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 ( )
SE3Tpl< typename MotionDerived::Scalar, typename MotionDerived::Vector3 ::Options > exp6(const MotionDense< MotionDerived > &nu)
Exp: se3 -> SE3.
SE3GroupAction< D >::ReturnType actInv(const D &d) const
by = aXb.actInv(ay)
static SE3Tpl Interpolate(const SE3Tpl &A, const SE3Tpl &B, const OtherScalar &alpha)
Linear interpolation on the SE3 manifold.