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);
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;
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;
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)
84 typedef typename Matrix3Like::Scalar Scalar;
85 typedef Eigen::Matrix<Scalar,3,1,
86 PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix3Like)::Options> Vector3;
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)
104 typename Matrix3Like::Scalar theta;
105 return log3(R.derived(),theta);
116 template<
typename Vector3Like,
typename Matrix3Like>
117 void Jexp3(
const Eigen::MatrixBase<Vector3Like> & r,
118 const Eigen::MatrixBase<Matrix3Like> & Jexp)
120 PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE (Vector3Like, r , 3, 1);
121 PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE (Matrix3Like, Jexp, 3, 3);
123 Matrix3Like & Jout = PINOCCHIO_EIGEN_CONST_CAST(Matrix3Like,Jexp);
124 typedef typename Matrix3Like::Scalar Scalar;
126 Scalar n2 = r.squaredNorm(),a,b,c;
127 Scalar n = math::sqrt(n2);
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);
137 Scalar n_inv = Scalar(1)/n;
138 Scalar n2_inv = n_inv * n_inv;
139 Scalar cn,sn;
SINCOS(n,&sn,&cn);
143 c = n2_inv * (1 - a);
146 Jout.diagonal().setConstant(a);
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);
152 Jout.noalias() += c * r * r.transpose();
167 template<
typename Scalar,
typename Vector3Like,
typename Matrix3Like>
169 const Eigen::MatrixBase<Vector3Like> & log,
170 const Eigen::MatrixBase<Matrix3Like> & Jlog)
173 PINOCCHIO_EIGEN_CONST_CAST(Matrix3Like,Jlog));
188 template<
typename Matrix3Like1,
typename Matrix3Like2>
189 void Jlog3(
const Eigen::MatrixBase<Matrix3Like1> & R,
190 const Eigen::MatrixBase<Matrix3Like2> & Jlog)
192 typedef typename Matrix3Like1::Scalar Scalar;
193 typedef Eigen::Matrix<Scalar,3,1,PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix3Like1)::Options> Vector3;
196 Vector3 w(
log3(R,t));
197 Jlog3(t,w,PINOCCHIO_EIGEN_CONST_CAST(Matrix3Like2,Jlog));
209 template<
typename MotionDerived>
213 typedef typename MotionDerived::Scalar Scalar;
214 enum { Options = PINOCCHIO_EIGEN_PLAIN_TYPE(
typename MotionDerived::Vector3)::Options };
219 typename SE3::LinearType & trans = res.translation();
220 typename SE3::AngularType & rot = res.rotation();
222 const typename MotionDerived::ConstAngularType & w = nu.angular();
223 const typename MotionDerived::ConstLinearType & v = nu.linear();
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;
232 Scalar(1)/Scalar(2) - t2/24,
233 (Scalar(1) - ct)*inv_t2);
240 (Scalar(1)/Scalar(6) - t2/120),
241 (Scalar(1) - alpha_v)*inv_t2);
248 trans.noalias() = (alpha_v*v + (alpha_w*w.dot(v))*w + alpha_wxv*w.cross(v));
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;
268 template<
typename Vector6Like>
270 exp6(
const Eigen::MatrixBase<Vector6Like> & v)
272 PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE (Vector6Like, v, 6, 1);
286 template<
typename Scalar,
int Options>
304 template<
typename Matrix4Like>
306 log6(
const Eigen::MatrixBase<Matrix4Like> & M)
308 PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix4Like, M, 4, 4);
310 typedef typename Matrix4Like::Scalar Scalar;
311 enum {Options = Eigen::internal::traits<Matrix4Like>::Options};
323 template<
typename MotionDerived,
typename Matrix6Like>
325 const Eigen::MatrixBase<Matrix6Like> & Jexp)
327 PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE (Matrix6Like, Jexp, 6, 6);
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);
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);
341 Jexp3(w, Jout.template bottomRightCorner<3,3>());
342 Jout.template topLeftCorner<3,3>() = Jout.template bottomRightCorner<3,3>();
344 Scalar beta, beta_dot_over_theta;
347 beta = Scalar(1)/Scalar(12) + t2/Scalar(720);
348 beta_dot_over_theta = Scalar(1)/Scalar(360);
352 const Scalar tinv = Scalar(1)/t,
354 Scalar st,ct;
SINCOS (t, &st, &ct);
355 const Scalar inv_2_2ct = Scalar(1)/(Scalar(2)*(Scalar(1)-ct));
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;
362 Vector3 p (Jout.template topLeftCorner<3,3>().transpose() * v);
363 Scalar wTp (w.dot (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());
370 Jout.template topRightCorner<3,3>().noalias() =
371 - Jout.template topLeftCorner<3,3>() * J;
372 Jout.template bottomLeftCorner<3,3>().setZero();
395 template<
typename Scalar,
int Options,
typename Matrix6Like>
397 const Eigen::MatrixBase<Matrix6Like> & Jlog)
402 template<
typename Scalar,
int Options>
403 template<
typename OtherScalar>
406 const OtherScalar & alpha)
412 ReturnType res = A *
exp6(alpha*dv);
418 #include "pinocchio/spatial/explog-quaternion.hpp" 419 #include "pinocchio/spatial/log.hxx" 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.
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.
SE3GroupAction< D >::ReturnType actInv(const D &d) const
by = aXb.actInv(ay)
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.
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.
static SE3Tpl Interpolate(const SE3Tpl &A, const SE3Tpl &B, const OtherScalar &alpha)
Linear interpolation on the SE3 manifold.