Program Listing for File explog.hpp
↰ Return to documentation for file (include/pinocchio/spatial/explog.hpp)
//
// Copyright (c) 2015-2023 CNRS INRIA
// Copyright (c) 2015 Wandercraft, 86 rue de Paris 91400 Orsay, France.
//
#ifndef __pinocchio_spatial_explog_hpp__
#define __pinocchio_spatial_explog_hpp__
#include "pinocchio/fwd.hpp"
#include "pinocchio/utils/static-if.hpp"
#include "pinocchio/math/fwd.hpp"
#include "pinocchio/math/sincos.hpp"
#include "pinocchio/math/taylor-expansion.hpp"
#include "pinocchio/spatial/motion.hpp"
#include "pinocchio/spatial/skew.hpp"
#include "pinocchio/spatial/se3.hpp"
#include <Eigen/Geometry>
#include "pinocchio/spatial/log.hpp"
namespace pinocchio
{
template<typename Vector3Like>
typename Eigen::Matrix<typename Vector3Like::Scalar,3,3,PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like)::Options>
exp3(const Eigen::MatrixBase<Vector3Like> & v)
{
PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE (Vector3Like, v, 3, 1);
typedef typename Vector3Like::Scalar Scalar;
typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like) Vector3LikePlain;
typedef Eigen::Matrix<Scalar,3,3,Vector3LikePlain::Options> Matrix3;
const Scalar t2 = v.squaredNorm();
const Scalar t = math::sqrt(t2);
Scalar ct,st; SINCOS(t,&st,&ct);
const Scalar alpha_vxvx = internal::if_then_else(internal::GT, t, TaylorSeriesExpansion<Scalar>::template precision<3>(),
(1 - ct)/t2,
Scalar(1)/Scalar(2) - t2/24);
const Scalar alpha_vx = internal::if_then_else(internal::GT, t, TaylorSeriesExpansion<Scalar>::template precision<3>(),
(st)/t,
Scalar(1) - t2/6);
Matrix3 res(alpha_vxvx * v * v.transpose());
res.coeffRef(0,1) -= alpha_vx * v[2]; res.coeffRef(1,0) += alpha_vx * v[2];
res.coeffRef(0,2) += alpha_vx * v[1]; res.coeffRef(2,0) -= alpha_vx * v[1];
res.coeffRef(1,2) -= alpha_vx * v[0]; res.coeffRef(2,1) += alpha_vx * v[0];
ct = internal::if_then_else(internal::GT, t, TaylorSeriesExpansion<Scalar>::template precision<3>(),
ct,
Scalar(1) - t2/2);
res.diagonal().array() += ct;
return res;
}
template<typename Matrix3Like>
Eigen::Matrix<typename Matrix3Like::Scalar,3,1,PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix3Like)::Options>
log3(const Eigen::MatrixBase<Matrix3Like> & R,
typename Matrix3Like::Scalar & theta)
{
typedef typename Matrix3Like::Scalar Scalar;
typedef Eigen::Matrix<Scalar,3,1,
PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix3Like)::Options> Vector3;
Vector3 res;
log3_impl<Scalar>::run(R, theta, res);
return res;
}
template<typename Matrix3Like>
Eigen::Matrix<typename Matrix3Like::Scalar,3,1,PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix3Like)::Options>
log3(const Eigen::MatrixBase<Matrix3Like> & R)
{
typename Matrix3Like::Scalar theta;
return log3(R.derived(),theta);
}
template<AssignmentOperatorType op, typename Vector3Like, typename Matrix3Like>
void Jexp3(const Eigen::MatrixBase<Vector3Like> & r,
const Eigen::MatrixBase<Matrix3Like> & Jexp)
{
PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE (Vector3Like, r , 3, 1);
PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE (Matrix3Like, Jexp, 3, 3);
Matrix3Like & Jout = PINOCCHIO_EIGEN_CONST_CAST(Matrix3Like,Jexp);
typedef typename Matrix3Like::Scalar Scalar;
const Scalar n2 = r.squaredNorm();
const Scalar n = math::sqrt(n2);
const Scalar n_inv = Scalar(1)/n;
const Scalar n2_inv = n_inv * n_inv;
Scalar cn,sn; SINCOS(n,&sn,&cn);
const Scalar a = internal::if_then_else(internal::LT, n, TaylorSeriesExpansion<Scalar>::template precision<3>(),
Scalar(1) - n2/Scalar(6),
sn*n_inv);
const Scalar b = internal::if_then_else(internal::LT, n, TaylorSeriesExpansion<Scalar>::template precision<3>(),
- Scalar(1)/Scalar(2) - n2/Scalar(24),
- (1-cn)*n2_inv);
const Scalar c = internal::if_then_else(internal::LT, n, TaylorSeriesExpansion<Scalar>::template precision<3>(),
Scalar(1)/Scalar(6) - n2/Scalar(120),
n2_inv * (1 - a));
switch(op)
{
case SETTO:
Jout.diagonal().setConstant(a);
Jout(0,1) = -b*r[2]; Jout(1,0) = -Jout(0,1);
Jout(0,2) = b*r[1]; Jout(2,0) = -Jout(0,2);
Jout(1,2) = -b*r[0]; Jout(2,1) = -Jout(1,2);
Jout.noalias() += c * r * r.transpose();
break;
case ADDTO:
Jout.diagonal().array() += a;
Jout(0,1) += -b*r[2]; Jout(1,0) += b*r[2];
Jout(0,2) += b*r[1]; Jout(2,0) += -b*r[1];
Jout(1,2) += -b*r[0]; Jout(2,1) += b*r[0];
Jout.noalias() += c * r * r.transpose();
break;
case RMTO:
Jout.diagonal().array() -= a;
Jout(0,1) -= -b*r[2]; Jout(1,0) -= b*r[2];
Jout(0,2) -= b*r[1]; Jout(2,0) -= -b*r[1];
Jout(1,2) -= -b*r[0]; Jout(2,1) -= b*r[0];
Jout.noalias() -= c * r * r.transpose();
break;
default:
assert(false && "Wrong Op requesed value");
break;
}
}
template<typename Vector3Like, typename Matrix3Like>
void Jexp3(const Eigen::MatrixBase<Vector3Like> & r,
const Eigen::MatrixBase<Matrix3Like> & Jexp)
{
Jexp3<SETTO>(r, Jexp);
}
template<typename Scalar, typename Vector3Like, typename Matrix3Like>
void Jlog3(const Scalar & theta,
const Eigen::MatrixBase<Vector3Like> & log,
const Eigen::MatrixBase<Matrix3Like> & Jlog)
{
Jlog3_impl<Scalar>::run(theta, log,
PINOCCHIO_EIGEN_CONST_CAST(Matrix3Like,Jlog));
}
template<typename Matrix3Like1, typename Matrix3Like2>
void Jlog3(const Eigen::MatrixBase<Matrix3Like1> & R,
const Eigen::MatrixBase<Matrix3Like2> & Jlog)
{
typedef typename Matrix3Like1::Scalar Scalar;
typedef Eigen::Matrix<Scalar,3,1,PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix3Like1)::Options> Vector3;
Scalar t;
Vector3 w(log3(R,t));
Jlog3(t,w,PINOCCHIO_EIGEN_CONST_CAST(Matrix3Like2,Jlog));
}
template<typename Scalar, typename Vector3Like1, typename Vector3Like2, typename Matrix3Like>
void Hlog3(const Scalar & theta,
const Eigen::MatrixBase<Vector3Like1> & log,
const Eigen::MatrixBase<Vector3Like2> & v,
const Eigen::MatrixBase<Matrix3Like> & vt_Hlog)
{
typedef Eigen::Matrix<Scalar,3,1,PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix3Like)::Options> Vector3;
Matrix3Like & vt_Hlog_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix3Like,vt_Hlog);
// theta = (log^T * log)^.5
// dt/dl = .5 * 2 * log^T * (log^T * log)^-.5
// = log^T / theta
// dt_dl = log / theta
Scalar ctheta,stheta; SINCOS(theta,&stheta,&ctheta);
Scalar denom = .5 / (1-ctheta),
a = theta * stheta * denom,
da_dt = (stheta - theta) * denom, // da / dtheta
b = ( 1 - a ) / (theta*theta),
//db_dt = - (2 * (1 - a) / theta + da_dt ) / theta**2; // db / dtheta
db_dt = - (2 / theta - (theta + stheta) * denom) / (theta*theta); // db / dtheta
// Compute dl_dv_v = Jlog * v
// Jlog = a I3 + .5 [ log ] + b * log * log^T
// if v == log, then Jlog * v == v
Vector3 dl_dv_v (a*v + .5*log.cross(v) + b*log*log.transpose()*v);
Scalar dt_dv_v = log.dot(dl_dv_v) / theta;
// Derivative of b * log * log^T
vt_Hlog_.noalias() = db_dt * dt_dv_v * log * log.transpose();
vt_Hlog_.noalias() += b * dl_dv_v * log.transpose();
vt_Hlog_.noalias() += b * log * dl_dv_v.transpose();
// Derivative of .5 * [ log ]
addSkew(.5 * dl_dv_v, vt_Hlog_);
// Derivative of a * I3
vt_Hlog_.diagonal().array() += da_dt * dt_dv_v;
}
template<typename Matrix3Like1, typename Vector3Like, typename Matrix3Like2>
void Hlog3(const Eigen::MatrixBase<Matrix3Like1> & R,
const Eigen::MatrixBase<Vector3Like> & v,
const Eigen::MatrixBase<Matrix3Like2> & vt_Hlog)
{
typedef typename Matrix3Like1::Scalar Scalar;
typedef Eigen::Matrix<Scalar,3,1,PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix3Like1)::Options> Vector3;
Scalar t;
Vector3 w(log3(R,t));
Hlog3(t,w,v,PINOCCHIO_EIGEN_CONST_CAST(Matrix3Like2,vt_Hlog));
}
template<typename MotionDerived>
SE3Tpl<typename MotionDerived::Scalar,PINOCCHIO_EIGEN_PLAIN_TYPE(typename MotionDerived::Vector3)::Options>
exp6(const MotionDense<MotionDerived> & nu)
{
typedef typename MotionDerived::Scalar Scalar;
enum { Options = PINOCCHIO_EIGEN_PLAIN_TYPE(typename MotionDerived::Vector3)::Options };
typedef SE3Tpl<Scalar,Options> SE3;
SE3 res;
typename SE3::LinearType & trans = res.translation();
typename SE3::AngularType & rot = res.rotation();
const typename MotionDerived::ConstAngularType & w = nu.angular();
const typename MotionDerived::ConstLinearType & v = nu.linear();
Scalar alpha_wxv, alpha_v, alpha_w, diagonal_term;
const Scalar t2 = w.squaredNorm();
const Scalar t = math::sqrt(t2);
Scalar ct,st; SINCOS(t,&st,&ct);
const Scalar inv_t2 = Scalar(1)/t2;
alpha_wxv = internal::if_then_else(internal::LT, t, TaylorSeriesExpansion<Scalar>::template precision<3>(),
Scalar(1)/Scalar(2) - t2/24,
(Scalar(1) - ct)*inv_t2);
alpha_v = internal::if_then_else(internal::LT, t, TaylorSeriesExpansion<Scalar>::template precision<3>(),
Scalar(1) - t2/6,
(st)/t);
alpha_w = internal::if_then_else(internal::LT, t, TaylorSeriesExpansion<Scalar>::template precision<3>(),
(Scalar(1)/Scalar(6) - t2/120),
(Scalar(1) - alpha_v)*inv_t2);
diagonal_term = internal::if_then_else(internal::LT, t, TaylorSeriesExpansion<Scalar>::template precision<3>(),
Scalar(1) - t2/2,
ct);
// Linear
trans.noalias() = (alpha_v*v + (alpha_w*w.dot(v))*w + alpha_wxv*w.cross(v));
// Rotational
rot.noalias() = alpha_wxv * w * w.transpose();
rot.coeffRef(0,1) -= alpha_v * w[2]; rot.coeffRef(1,0) += alpha_v * w[2];
rot.coeffRef(0,2) += alpha_v * w[1]; rot.coeffRef(2,0) -= alpha_v * w[1];
rot.coeffRef(1,2) -= alpha_v * w[0]; rot.coeffRef(2,1) += alpha_v * w[0];
rot.diagonal().array() += diagonal_term;
return res;
}
template<typename Vector6Like>
SE3Tpl<typename Vector6Like::Scalar,PINOCCHIO_EIGEN_PLAIN_TYPE(Vector6Like)::Options>
exp6(const Eigen::MatrixBase<Vector6Like> & v)
{
PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE (Vector6Like, v, 6, 1);
MotionRef<const Vector6Like> nu(v.derived());
return exp6(nu);
}
template<typename Scalar, int Options>
MotionTpl<Scalar,Options>
log6(const SE3Tpl<Scalar,Options> & M)
{
typedef MotionTpl<Scalar,Options> Motion;
Motion mout;
log6_impl<Scalar>::run(M, mout);
return mout;
}
template<typename Matrix4Like>
MotionTpl<typename Matrix4Like::Scalar,Eigen::internal::traits<Matrix4Like>::Options>
log6(const Eigen::MatrixBase<Matrix4Like> & M)
{
PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix4Like, M, 4, 4);
typedef typename Matrix4Like::Scalar Scalar;
enum {Options = Eigen::internal::traits<Matrix4Like>::Options};
typedef MotionTpl<Scalar,Options> Motion;
typedef SE3Tpl<Scalar,Options> SE3;
SE3 m(M);
Motion mout;
log6_impl<Scalar>::run(m, mout);
return mout;
}
template<AssignmentOperatorType op, typename MotionDerived, typename Matrix6Like>
void Jexp6(const MotionDense<MotionDerived> & nu,
const Eigen::MatrixBase<Matrix6Like> & Jexp)
{
PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE (Matrix6Like, Jexp, 6, 6);
typedef typename MotionDerived::Scalar Scalar;
typedef typename MotionDerived::Vector3 Vector3;
typedef Eigen::Matrix<Scalar, 3, 3, Vector3::Options> Matrix3;
Matrix6Like & Jout = PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like,Jexp);
const typename MotionDerived::ConstLinearType & v = nu.linear();
const typename MotionDerived::ConstAngularType & w = nu.angular();
const Scalar t2 = w.squaredNorm();
const Scalar t = math::sqrt(t2);
const Scalar tinv = Scalar(1)/t,
t2inv = tinv*tinv;
Scalar st,ct; SINCOS (t, &st, &ct);
const Scalar inv_2_2ct = Scalar(1)/(Scalar(2)*(Scalar(1)-ct));
const Scalar beta = internal::if_then_else(internal::LT, t, TaylorSeriesExpansion<Scalar>::template precision<3>(),
Scalar(1)/Scalar(12) + t2/Scalar(720),
t2inv - st*tinv*inv_2_2ct);
const Scalar beta_dot_over_theta = internal::if_then_else(internal::LT, t, TaylorSeriesExpansion<Scalar>::template precision<3>(),
Scalar(1)/Scalar(360),
-Scalar(2)*t2inv*t2inv + (Scalar(1) + st*tinv) * t2inv * inv_2_2ct);
switch(op)
{
case SETTO:
{
Jexp3<SETTO>(w, Jout.template bottomRightCorner<3,3>());
Jout.template topLeftCorner<3,3>() = Jout.template bottomRightCorner<3,3>();
const Vector3 p = Jout.template topLeftCorner<3,3>().transpose() * v;
const Scalar wTp (w.dot (p));
const Matrix3 J (alphaSkew(.5, p) +
(beta_dot_over_theta*wTp) *w*w.transpose()
- (t2*beta_dot_over_theta+Scalar(2)*beta)*p*w.transpose()
+ wTp * beta * Matrix3::Identity()
+ beta *w*p.transpose());
Jout.template topRightCorner<3,3>().noalias() =
- Jout.template topLeftCorner<3,3>() * J;
Jout.template bottomLeftCorner<3,3>().setZero();
break;
}
case ADDTO:
{
PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED
Matrix3 Jtmp3;
Jexp3<SETTO>(w, Jtmp3);
PINOCCHIO_COMPILER_DIAGNOSTIC_POP
Jout.template bottomRightCorner<3,3>() += Jtmp3;
Jout.template topLeftCorner<3,3>() += Jtmp3;
const Vector3 p = Jtmp3.transpose() * v;
const Scalar wTp (w.dot (p));
const Matrix3 J (alphaSkew(.5, p) +
(beta_dot_over_theta*wTp) *w*w.transpose()
- (t2*beta_dot_over_theta+Scalar(2)*beta)*p*w.transpose()
+ wTp * beta * Matrix3::Identity()
+ beta *w*p.transpose());
Jout.template topRightCorner<3,3>().noalias() +=
- Jtmp3 * J;
break;
}
case RMTO:
{
PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED
Matrix3 Jtmp3;
Jexp3<SETTO>(w, Jtmp3);
PINOCCHIO_COMPILER_DIAGNOSTIC_POP
Jout.template bottomRightCorner<3,3>() -= Jtmp3;
Jout.template topLeftCorner<3,3>() -= Jtmp3;
const Vector3 p = Jtmp3.transpose() * v;
const Scalar wTp (w.dot (p));
const Matrix3 J (alphaSkew(.5, p) +
(beta_dot_over_theta*wTp) *w*w.transpose()
- (t2*beta_dot_over_theta+Scalar(2)*beta)*p*w.transpose()
+ wTp * beta * Matrix3::Identity()
+ beta *w*p.transpose());
Jout.template topRightCorner<3,3>().noalias() -=
- Jtmp3 * J;
break;
}
default:
assert(false && "Wrong Op requesed value");
break;
}
}
template<typename MotionDerived, typename Matrix6Like>
void Jexp6(const MotionDense<MotionDerived> & nu,
const Eigen::MatrixBase<Matrix6Like> & Jexp)
{
Jexp6<SETTO>(nu, Jexp);
}
template<typename Scalar, int Options, typename Matrix6Like>
void Jlog6(const SE3Tpl<Scalar, Options> & M,
const Eigen::MatrixBase<Matrix6Like> & Jlog)
{
Jlog6_impl<Scalar>::run(M,PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like,Jlog));
}
template<typename Scalar, int Options>
template<typename OtherScalar>
SE3Tpl<Scalar,Options> SE3Tpl<Scalar,Options>::Interpolate(const SE3Tpl & A,
const SE3Tpl & B,
const OtherScalar & alpha)
{
typedef SE3Tpl<Scalar,Options> ReturnType;
typedef MotionTpl<Scalar,Options> Motion;
Motion dv = log6(A.actInv(B));
ReturnType res = A * exp6(alpha*dv);
return res;
}
} // namespace pinocchio
#include "pinocchio/spatial/explog-quaternion.hpp"
#include "pinocchio/spatial/log.hxx"
#endif //#ifndef __pinocchio_spatial_explog_hpp__