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"
33 template<
typename Vector3Like>
35 Matrix<
typename Vector3Like::Scalar, 3, 3, PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like)::Options>
38 PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE(Vector3Like, v, 3, 1);
40 typedef typename Vector3Like::Scalar Scalar;
42 typedef Eigen::Matrix<Scalar, 3, 3, Vector3LikePlain::Options> Matrix3;
43 const static Scalar
eps = Eigen::NumTraits<Scalar>::epsilon();
45 const Scalar
t2 = v.squaredNorm() +
eps *
eps;
47 const Scalar
t = math::sqrt(
t2);
51 const Scalar
alpha_vxvx = internal::if_then_else(
53 static_cast<Scalar
>((1 -
ct) /
t2),
static_cast<Scalar
>(Scalar(1) / Scalar(2) -
t2 / 24));
54 const Scalar
alpha_vx = internal::if_then_else(
56 static_cast<Scalar
>((
st) /
t),
static_cast<Scalar
>(Scalar(1) -
t2 / 6));
58 res.coeffRef(0, 1) -=
alpha_vx * v[2];
59 res.coeffRef(1, 0) +=
alpha_vx * v[2];
60 res.coeffRef(0, 2) +=
alpha_vx * v[1];
61 res.coeffRef(2, 0) -=
alpha_vx * v[1];
62 res.coeffRef(1, 2) -=
alpha_vx * v[0];
63 res.coeffRef(2, 1) +=
alpha_vx * v[0];
65 ct = internal::if_then_else(
67 static_cast<Scalar
>(Scalar(1) -
t2 / 2));
68 res.diagonal().array() +=
ct;
80 template<
typename Matrix3Like>
82 Matrix<
typename Matrix3Like::Scalar, 3, 1, PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix3Like)::Options>
85 typedef typename Matrix3Like::Scalar Scalar;
86 typedef Eigen::Matrix<Scalar, 3, 1, PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix3Like)::Options> Vector3;
100 template<
typename Matrix3Like>
102 Matrix<
typename Matrix3Like::Scalar, 3, 1, PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix3Like)::Options>
105 typename Matrix3Like::Scalar
theta;
117 template<AssignmentOperatorType op,
typename Vector3Like,
typename Matrix3Like>
118 void Jexp3(
const Eigen::MatrixBase<Vector3Like> & r,
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 const Scalar
n2 = r.squaredNorm();
127 const Scalar
n = math::sqrt(
n2);
128 const Scalar
n_inv = Scalar(1) /
n;
133 const Scalar
a = internal::if_then_else(
135 static_cast<Scalar
>(Scalar(1) -
n2 / Scalar(6)),
static_cast<Scalar
>(
sn *
n_inv));
136 const Scalar b = internal::if_then_else(
138 static_cast<Scalar
>(-Scalar(1) / Scalar(2) -
n2 / Scalar(24)),
139 static_cast<Scalar
>(-(1 -
cn) *
n2_inv));
140 const Scalar c = internal::if_then_else(
142 static_cast<Scalar
>(Scalar(1) / Scalar(6) -
n2 / Scalar(120)),
143 static_cast<Scalar
>(
n2_inv * (1 -
a)));
148 Jout.diagonal().setConstant(
a);
149 Jout(0, 1) = -b * r[2];
151 Jout(0, 2) = b * r[1];
153 Jout(1, 2) = -b * r[0];
155 Jout.noalias() += c * r * r.transpose();
158 Jout.diagonal().array() +=
a;
159 Jout(0, 1) += -b * r[2];
160 Jout(1, 0) += b * r[2];
161 Jout(0, 2) += b * r[1];
162 Jout(2, 0) += -b * r[1];
163 Jout(1, 2) += -b * r[0];
164 Jout(2, 1) += b * r[0];
165 Jout.noalias() += c * r * r.transpose();
168 Jout.diagonal().array() -=
a;
169 Jout(0, 1) -= -b * r[2];
170 Jout(1, 0) -= b * r[2];
171 Jout(0, 2) -= b * r[1];
172 Jout(2, 0) -= -b * r[1];
173 Jout(1, 2) -= -b * r[0];
174 Jout(2, 1) -= b * r[0];
175 Jout.noalias() -= c * r * r.transpose();
178 assert(
false &&
"Wrong Op requesed value");
191 template<
typename Vector3Like,
typename Matrix3Like>
192 void Jexp3(
const Eigen::MatrixBase<Vector3Like> & r,
const Eigen::MatrixBase<Matrix3Like> &
Jexp)
239 template<
typename Scalar,
typename Vector3Like,
typename Matrix3Like>
241 const Scalar &
theta,
242 const Eigen::MatrixBase<Vector3Like> & log,
243 const Eigen::MatrixBase<Matrix3Like> & Jlog)
260 template<
typename Matrix3Like1,
typename Matrix3Like2>
262 Jlog3(
const Eigen::MatrixBase<Matrix3Like1> & R,
const Eigen::MatrixBase<Matrix3Like2> & Jlog)
264 typedef typename Matrix3Like1::Scalar Scalar;
265 typedef Eigen::Matrix<Scalar, 3, 1, PINOCCHIO_EIGEN_PLAIN_TYPE(
Matrix3Like1)::Options> Vector3;
268 Vector3 w(
log3(R,
t));
272 template<
typename Scalar,
typename Vector3Like1,
typename Vector3Like2,
typename Matrix3Like>
274 const Scalar & theta,
275 const Eigen::MatrixBase<Vector3Like1> & log,
276 const Eigen::MatrixBase<Vector3Like2> & v,
277 const Eigen::MatrixBase<Matrix3Like> & vt_Hlog)
279 typedef Eigen::Matrix<Scalar, 3, 1, PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix3Like)::Options> Vector3;
280 Matrix3Like & vt_Hlog_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix3Like, vt_Hlog);
286 Scalar ctheta, stheta;
287 SINCOS(theta, &stheta, &ctheta);
289 Scalar denom = .5 / (1 - ctheta), a = theta * stheta * denom,
290 da_dt = (stheta - theta) * denom,
291 b = (1 - a) / (theta * theta),
293 db_dt = -(2 / theta - (theta + stheta) * denom) / (theta * theta);
298 Vector3 dl_dv_v(a * v + .5 * log.cross(v) + b * log * log.transpose() * v);
300 Scalar dt_dv_v = log.dot(dl_dv_v) / theta;
303 vt_Hlog_.noalias() = db_dt * dt_dv_v * log * log.transpose();
304 vt_Hlog_.noalias() += b * dl_dv_v * log.transpose();
305 vt_Hlog_.noalias() += b * log * dl_dv_v.transpose();
307 addSkew(.5 * dl_dv_v, vt_Hlog_);
309 vt_Hlog_.diagonal().array() += da_dt * dt_dv_v;
320 template<
typename Matrix3Like1,
typename Vector3Like,
typename Matrix3Like2>
322 const Eigen::MatrixBase<Matrix3Like1> & R,
323 const Eigen::MatrixBase<Vector3Like> & v,
324 const Eigen::MatrixBase<Matrix3Like2> &
vt_Hlog)
326 typedef typename Matrix3Like1::Scalar Scalar;
327 typedef Eigen::Matrix<Scalar, 3, 1, PINOCCHIO_EIGEN_PLAIN_TYPE(
Matrix3Like1)::Options> Vector3;
330 Vector3 w(
log3(R,
t));
343 template<
typename MotionDerived>
345 typename MotionDerived::Scalar,
346 PINOCCHIO_EIGEN_PLAIN_TYPE(
typename MotionDerived::Vector3)::Options>
349 typedef typename MotionDerived::Scalar Scalar;
352 Options = PINOCCHIO_EIGEN_PLAIN_TYPE(
typename MotionDerived::Vector3)::Options
358 typename SE3::LinearType & trans = res.translation();
359 typename SE3::AngularType & rot = res.rotation();
361 const typename MotionDerived::ConstAngularType & w = nu.angular();
362 const typename MotionDerived::ConstLinearType & v = nu.linear();
363 const static Scalar
eps = Eigen::NumTraits<Scalar>::epsilon();
366 const Scalar
t2 = w.squaredNorm() +
eps *
eps;
367 const Scalar
t = math::sqrt(
t2);
370 const Scalar
inv_t2 = Scalar(1) /
t2;
374 static_cast<Scalar
>(Scalar(1) / Scalar(2) -
t2 / 24),
375 static_cast<Scalar
>((Scalar(1) -
ct) *
inv_t2));
377 alpha_v = internal::if_then_else(
379 static_cast<Scalar
>(Scalar(1) -
t2 / 6),
static_cast<Scalar
>((
st) /
t));
381 alpha_w = internal::if_then_else(
383 static_cast<Scalar
>((Scalar(1) / Scalar(6) -
t2 / 120)),
388 static_cast<Scalar
>(Scalar(1) -
t2 / 2),
ct);
394 rot.noalias() =
alpha_wxv * w * w.transpose();
395 rot.coeffRef(0, 1) -=
alpha_v * w[2];
396 rot.coeffRef(1, 0) +=
alpha_v * w[2];
397 rot.coeffRef(0, 2) +=
alpha_v * w[1];
398 rot.coeffRef(2, 0) -=
alpha_v * w[1];
399 rot.coeffRef(1, 2) -=
alpha_v * w[0];
400 rot.coeffRef(2, 1) +=
alpha_v * w[0];
415 template<
typename Vector6Like>
416 SE3Tpl<
typename Vector6Like::Scalar, PINOCCHIO_EIGEN_PLAIN_TYPE(Vector6Like)::Options>
419 PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE(
Vector6Like, v, 6, 1);
434 template<
typename Scalar,
int Options>
453 template<
typename Vector3Like,
typename QuaternionLike>
455 const Eigen::QuaternionBase<QuaternionLike> &
quat,
const Eigen::MatrixBase<Vector3Like> &
vec)
457 typedef typename Vector3Like::Scalar Scalar;
473 template<
typename Matrix4Like>
474 MotionTpl<typename Matrix4Like::Scalar, Eigen::internal::traits<Matrix4Like>::Options>
475 log6(
const Eigen::MatrixBase<Matrix4Like> & M)
477 PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE(
Matrix4Like, M, 4, 4);
479 typedef typename Matrix4Like::Scalar Scalar;
482 Options = Eigen::internal::traits<Matrix4Like>::Options
495 template<AssignmentOperatorType op,
typename MotionDerived,
typename Matrix6Like>
500 typedef typename MotionDerived::Scalar Scalar;
501 typedef typename MotionDerived::Vector3 Vector3;
502 typedef Eigen::Matrix<Scalar, 3, 3, Vector3::Options> Matrix3;
505 const typename MotionDerived::ConstLinearType & v = nu.linear();
506 const typename MotionDerived::ConstAngularType & w = nu.angular();
507 const Scalar
t2 = w.squaredNorm();
508 const Scalar
t = math::sqrt(
t2);
513 const Scalar
inv_2_2ct = Scalar(1) / (Scalar(2) * (Scalar(1) -
ct));
515 const Scalar
beta = internal::if_then_else(
517 static_cast<Scalar
>(Scalar(1) / Scalar(12) +
t2 / Scalar(720)),
522 static_cast<Scalar
>(Scalar(1) / Scalar(360)),
532 const Scalar
wTp(w.dot(p));
536 +
wTp *
beta * Matrix3::Identity() +
beta * w * p.transpose());
542 PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
543 PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED
546 PINOCCHIO_COMPILER_DIAGNOSTIC_POP
549 const Vector3 p =
Jtmp3.transpose() * v;
550 const Scalar
wTp(w.dot(p));
554 +
wTp *
beta * Matrix3::Identity() +
beta * w * p.transpose());
559 PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
560 PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED
563 PINOCCHIO_COMPILER_DIAGNOSTIC_POP
566 const Vector3 p =
Jtmp3.transpose() * v;
567 const Scalar
wTp(w.dot(p));
571 +
wTp *
beta * Matrix3::Identity() +
beta * w * p.transpose());
576 assert(
false &&
"Wrong Op requesed value");
583 template<
typename MotionDerived,
typename Matrix6Like>
591 template<
typename MotionDerived>
592 Eigen::Matrix<typename MotionDerived::Scalar, 6, 6, MotionDerived::Options>
595 typedef typename MotionDerived::Scalar Scalar;
598 Options = MotionDerived::Options
600 typedef Eigen::Matrix<Scalar, 6, 6, Options> ReturnType;
667 template<
typename Scalar,
int Options,
typename Matrix6Like>
678 template<
typename Scalar,
int Options>
681 typedef Eigen::Matrix<Scalar, 6, 6, Options> ReturnType;
688 template<
typename Scalar,
int Options>
689 template<
typename OtherScalar>
691 const SE3Tpl & A,
const SE3Tpl & B,
const OtherScalar & alpha)
693 typedef SE3Tpl<Scalar, Options> ReturnType;
694 typedef MotionTpl<Scalar, Options> Motion;
696 Motion dv =
log6(A.actInv(B));
697 ReturnType res = A *
exp6(alpha * dv);
703#include "pinocchio/spatial/explog-quaternion.hpp"
704#include "pinocchio/spatial/log.hxx"
Main pinocchio namespace.
void Jlog3(const Scalar &theta, const Eigen::MatrixBase< Vector3Like > &log, const Eigen::MatrixBase< Matrix3Like > &Jlog)
Derivative of log3.
void SINCOS(const S1 &a, S2 *sa, S3 *ca)
Computes sin/cos values of a given input scalar.
Eigen::Matrix< typename Matrix3Like::Scalar, 3, 1, Matrix3Like ::Options > log3(const Eigen::MatrixBase< Matrix3Like > &R, typename Matrix3Like::Scalar &theta)
Same as log3.
Eigen::Matrix< typename Vector3Like::Scalar, 3, 3, Vector3Like ::Options > exp3(const Eigen::MatrixBase< Vector3Like > &v)
Exp: so3 -> SO3.
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...
MotionTpl< Scalar, Options > log6(const SE3Tpl< Scalar, Options > &M)
Log: SE3 -> se3.
SE3Tpl< typename MotionDerived::Scalar, typename MotionDerived::Vector3 ::Options > exp6(const MotionDense< MotionDerived > &nu)
Exp: se3 -> SE3.
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....
void Jlog6(const SE3Tpl< Scalar, Options > &M, const Eigen::MatrixBase< Matrix6Like > &Jlog)
Derivative of log6.
void Jexp6(const MotionDense< MotionDerived > &nu, const Eigen::MatrixBase< Matrix6Like > &Jexp)
Derivative of exp6 Computed as the inverse of Jlog6.
void Jexp3(const Eigen::MatrixBase< Vector3Like > &r, const Eigen::MatrixBase< Matrix3Like > &Jexp)
Derivative of .
static SE3Tpl Interpolate(const SE3Tpl &A, const SE3Tpl &B, const OtherScalar &alpha)
Linear interpolation on the SE3 manifold.