5 #ifndef __pinocchio_multibody_liegroup_special_euclidean_operation_hpp__
6 #define __pinocchio_multibody_liegroup_special_euclidean_operation_hpp__
10 #include "pinocchio/macros.hpp"
11 #include "pinocchio/math/quaternion.hpp"
12 #include "pinocchio/math/matrix.hpp"
13 #include "pinocchio/spatial/fwd.hpp"
14 #include "pinocchio/utils/static-if.hpp"
15 #include "pinocchio/spatial/se3.hpp"
16 #include "pinocchio/multibody/liegroup/liegroup-base.hpp"
18 #include "pinocchio/multibody/liegroup/vector-space.hpp"
19 #include "pinocchio/multibody/liegroup/cartesian-product.hpp"
20 #include "pinocchio/multibody/liegroup/special-orthogonal.hpp"
24 template<
int Dim,
typename Scalar,
int Options = 0>
29 template<
int Dim,
typename Scalar,
int Options>
34 template<
typename _Scalar,
int _Options>
37 typedef _Scalar Scalar;
47 template<
typename _Scalar,
int _Options>
49 :
public LieGroupBase<SpecialEuclideanOperationTpl<2, _Scalar, _Options>>
57 typedef Eigen::Matrix<Scalar, 2, 2, Options> Matrix2;
58 typedef Eigen::Matrix<Scalar, 2, 1, Options> Vector2;
60 template<
typename TangentVector,
typename Matrix2Like,
typename Vector2Like>
62 const Eigen::MatrixBase<TangentVector> & v,
63 const Eigen::MatrixBase<Matrix2Like> & R,
64 const Eigen::MatrixBase<Vector2Like> & t)
66 EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(TangentVector_t, TangentVector);
67 EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix2Like, 2, 2);
68 EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector2Like, 2);
70 typedef typename Matrix2Like::Scalar Scalar;
71 const Scalar omega = v(2);
74 PINOCCHIO_EIGEN_CONST_CAST(Matrix2Like, R) << cv, -sv, sv, cv;
75 using internal::if_then_else;
78 typename PINOCCHIO_EIGEN_PLAIN_TYPE(Vector2Like) vcross(-v(1), v(0));
79 vcross -= -v(1) * R.col(0) + v(0) * R.col(1);
81 Scalar omega_abs = math::fabs(omega);
82 PINOCCHIO_EIGEN_CONST_CAST(Vector2Like, t).coeffRef(0) = if_then_else(
83 internal::GT, omega_abs, Scalar(1e-14),
84 vcross.coeff(0), v.coeff(0));
86 PINOCCHIO_EIGEN_CONST_CAST(Vector2Like, t).coeffRef(1) = if_then_else(
87 internal::GT, omega_abs, Scalar(1e-14),
88 vcross.coeff(1), v.coeff(1));
92 template<
typename Matrix2Like,
typename Vector2Like,
typename Matrix3Like>
93 static void toInverseActionMatrix(
94 const Eigen::MatrixBase<Matrix2Like> & R,
95 const Eigen::MatrixBase<Vector2Like> & t,
96 const Eigen::MatrixBase<Matrix3Like> & M,
97 const AssignmentOperatorType op)
99 EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix2Like, 2, 2);
100 EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector2Like, 2);
101 PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix3Like, M, 3, 3);
102 Matrix3Like & Mout = PINOCCHIO_EIGEN_CONST_CAST(Matrix3Like, M);
103 typedef typename Matrix3Like::Scalar Scalar;
105 typename PINOCCHIO_EIGEN_PLAIN_TYPE(Vector2Like) tinv((R.transpose() * t).reverse());
106 tinv[0] *= Scalar(-1.);
110 Mout.template topLeftCorner<2, 2>() = R.transpose();
111 Mout.template topRightCorner<2, 1>() = tinv;
112 Mout.template bottomLeftCorner<1, 2>().setZero();
113 Mout(2, 2) = (Scalar)1;
116 Mout.template topLeftCorner<2, 2>() += R.transpose();
117 Mout.template topRightCorner<2, 1>() += tinv;
118 Mout(2, 2) += (Scalar)1;
121 Mout.template topLeftCorner<2, 2>() -= R.transpose();
122 Mout.template topRightCorner<2, 1>() -= tinv;
123 Mout(2, 2) -= (Scalar)1;
126 assert(
false &&
"Wrong Op requesed value");
131 template<
typename Matrix2Like,
typename Vector2Like,
typename TangentVector>
133 const Eigen::MatrixBase<Matrix2Like> & R,
134 const Eigen::MatrixBase<Vector2Like> & p,
135 const Eigen::MatrixBase<TangentVector> & v)
137 EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix2Like, 2, 2);
138 EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector2Like, 2);
139 EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(TangentVector_t, TangentVector);
141 TangentVector & vout = PINOCCHIO_EIGEN_CONST_CAST(TangentVector, v);
143 typedef typename Matrix2Like::Scalar Scalar1;
145 Scalar1 t = SO2_t::log(R);
146 const Scalar1 tabs = math::fabs(t);
147 const Scalar1 t2 = t * t;
151 alpha = internal::if_then_else(
152 internal::LT, tabs, Scalar(1e-4),
153 static_cast<Scalar
>(1 - t2 / 12 - t2 * t2 / 720),
154 static_cast<Scalar
>(tabs * st / (2 * (1 - ct))));
156 vout.template head<2>().noalias() = alpha * p;
157 vout(0) += t / 2 * p(1);
158 vout(1) += -t / 2 * p(0);
162 template<
typename Matrix2Like,
typename Vector2Like,
typename JacobianOutLike>
164 const Eigen::MatrixBase<Matrix2Like> & R,
165 const Eigen::MatrixBase<Vector2Like> & p,
166 const Eigen::MatrixBase<JacobianOutLike> & J)
168 EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix2Like, 2, 2);
169 EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector2Like, 2);
170 EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(JacobianOutLike, JacobianMatrix_t);
172 JacobianOutLike & Jout = PINOCCHIO_EIGEN_CONST_CAST(JacobianOutLike, J);
174 typedef typename Matrix2Like::Scalar Scalar1;
176 Scalar1 t = SO2_t::log(R);
177 const Scalar1 tabs = math::fabs(t);
178 Scalar1 alpha, alpha_dot;
182 Scalar1 inv_2_1_ct = 0.5 / (1 - ct);
184 alpha = internal::if_then_else(
185 internal::LT, tabs, Scalar(1e-4),
static_cast<Scalar
>(1 - t2 / 12),
186 static_cast<Scalar
>(t * st * inv_2_1_ct));
187 alpha_dot = internal::if_then_else(
188 internal::LT, tabs, Scalar(1e-4),
static_cast<Scalar
>(-t / 6 - t2 * t / 180),
189 static_cast<Scalar
>((st - t) * inv_2_1_ct));
191 typename PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix2Like) V;
192 V(0, 0) = V(1, 1) = alpha;
196 Jout.template topLeftCorner<2, 2>().noalias() = V * R;
197 Jout.template topRightCorner<2, 1>() << alpha_dot * p[0] + p[1] / 2,
198 -p(0) / 2 + alpha_dot * p(1);
199 Jout.template bottomLeftCorner<1, 2>().setZero();
217 static ConfigVector_t
neutral()
220 n << Scalar(0), Scalar(0), Scalar(1), Scalar(0);
224 static std::string
name()
226 return std::string(
"SE(2)");
229 template<
class ConfigL_t,
class ConfigR_t,
class Tangent_t>
230 static void difference_impl(
231 const Eigen::MatrixBase<ConfigL_t> & q0,
232 const Eigen::MatrixBase<ConfigR_t> & q1,
233 const Eigen::MatrixBase<Tangent_t> & d)
235 PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
236 PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED
241 PINOCCHIO_COMPILER_DIAGNOSTIC_POP
242 Matrix2 R(R0.transpose() * R1);
243 Vector2 t(R0.transpose() * (t1 - t0));
248 template<ArgumentPosition arg,
class ConfigL_t,
class ConfigR_t,
class JacobianOut_t>
249 void dDifference_impl(
250 const Eigen::MatrixBase<ConfigL_t> & q0,
251 const Eigen::MatrixBase<ConfigR_t> & q1,
252 const Eigen::MatrixBase<JacobianOut_t> & J)
const
254 PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
255 PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED
260 PINOCCHIO_COMPILER_DIAGNOSTIC_POP
261 Matrix2 R(R0.transpose() * R1);
262 Vector2 t(R0.transpose() * (t1 - t0));
266 PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
267 PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED
270 PINOCCHIO_COMPILER_DIAGNOSTIC_POP
273 Vector2 pcross(q1(1) - q0(1), q0(0) - q1(0));
275 JacobianOut_t & J0 = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, J);
276 J0.template topLeftCorner<2, 2>().noalias() = -R.transpose();
277 J0.template topRightCorner<2, 1>().noalias() = R1.transpose() * pcross;
278 J0.template bottomLeftCorner<1, 2>().setZero();
280 J0.applyOnTheLeft(J1);
282 else if (arg == ARG1)
288 template<
class ConfigIn_t,
class Velocity_t,
class ConfigOut_t>
289 static void integrate_impl(
290 const Eigen::MatrixBase<ConfigIn_t> & q,
291 const Eigen::MatrixBase<Velocity_t> & v,
292 const Eigen::MatrixBase<ConfigOut_t> & qout)
294 ConfigOut_t & out = PINOCCHIO_EIGEN_CONST_CAST(ConfigOut_t, qout);
296 PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
297 PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED
302 PINOCCHIO_COMPILER_DIAGNOSTIC_POP
304 out.template head<2>().noalias() = R0 * t + t0;
305 out.template tail<2>().noalias() = R0 * R.col(0);
308 template<
class Config_t,
class Jacobian_t>
309 static void integrateCoeffWiseJacobian_impl(
310 const Eigen::MatrixBase<Config_t> & q,
const Eigen::MatrixBase<Jacobian_t> & J)
312 assert(J.rows() ==
nq() && J.cols() ==
nv() &&
"J is not of the right dimension");
314 Jacobian_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t, J);
317 const typename Config_t::Scalar &c_theta = q(2), &s_theta = q(3);
319 Jout.template topLeftCorner<2, 2>() << c_theta, -s_theta, s_theta, c_theta;
320 Jout.template bottomRightCorner<2, 1>() << -s_theta, c_theta;
323 template<
class Config_t,
class Tangent_t,
class JacobianOut_t>
324 static void dIntegrate_dq_impl(
325 const Eigen::MatrixBase<Config_t> & ,
326 const Eigen::MatrixBase<Tangent_t> & v,
327 const Eigen::MatrixBase<JacobianOut_t> & J,
328 const AssignmentOperatorType op = SETTO)
330 JacobianOut_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, J);
332 PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
333 PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED
337 PINOCCHIO_COMPILER_DIAGNOSTIC_POP
339 toInverseActionMatrix(R, t, Jout, op);
342 template<
class Config_t,
class Tangent_t,
class JacobianOut_t>
343 static void dIntegrate_dv_impl(
344 const Eigen::MatrixBase<Config_t> & ,
345 const Eigen::MatrixBase<Tangent_t> & v,
346 const Eigen::MatrixBase<JacobianOut_t> & J,
347 const AssignmentOperatorType op = SETTO)
349 JacobianOut_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, J);
351 MotionTpl<Scalar, 0> nu;
352 nu.toVector() << v.template head<2>(), 0, 0, 0, v[2];
353 PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
354 PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED
355 Eigen::Matrix<Scalar, 6, 6> Jtmp6;
357 PINOCCHIO_COMPILER_DIAGNOSTIC_POP
362 Jout << Jtmp6.template topLeftCorner<2, 2>(), Jtmp6.template topRightCorner<2, 1>(),
363 Jtmp6.template bottomLeftCorner<1, 2>(), Jtmp6.template bottomRightCorner<1, 1>();
366 Jout.template topLeftCorner<2, 2>() += Jtmp6.template topLeftCorner<2, 2>();
367 Jout.template topRightCorner<2, 1>() += Jtmp6.template topRightCorner<2, 1>();
368 Jout.template bottomLeftCorner<1, 2>() += Jtmp6.template bottomLeftCorner<1, 2>();
369 Jout.template bottomRightCorner<1, 1>() += Jtmp6.template bottomRightCorner<1, 1>();
372 Jout.template topLeftCorner<2, 2>() -= Jtmp6.template topLeftCorner<2, 2>();
373 Jout.template topRightCorner<2, 1>() -= Jtmp6.template topRightCorner<2, 1>();
374 Jout.template bottomLeftCorner<1, 2>() -= Jtmp6.template bottomLeftCorner<1, 2>();
375 Jout.template bottomRightCorner<1, 1>() -= Jtmp6.template bottomRightCorner<1, 1>();
378 assert(
false &&
"Wrong Op requesed value");
383 template<
class Config_t,
class Tangent_t,
class JacobianIn_t,
class JacobianOut_t>
384 static void dIntegrateTransport_dq_impl(
385 const Eigen::MatrixBase<Config_t> & ,
386 const Eigen::MatrixBase<Tangent_t> & v,
387 const Eigen::MatrixBase<JacobianIn_t> & Jin,
388 const Eigen::MatrixBase<JacobianOut_t> & J_out)
390 JacobianOut_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, J_out);
391 PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
392 PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED
396 PINOCCHIO_COMPILER_DIAGNOSTIC_POP
398 Vector2 tinv = (R.transpose() * t).reverse();
399 tinv[0] *= Scalar(-1.);
401 Jout.template topRows<2>().noalias() = R.transpose() * Jin.template topRows<2>();
402 Jout.template topRows<2>().noalias() += tinv * Jin.template bottomRows<1>();
403 Jout.template bottomRows<1>() = Jin.template bottomRows<1>();
406 template<
class Config_t,
class Tangent_t,
class JacobianIn_t,
class JacobianOut_t>
407 static void dIntegrateTransport_dv_impl(
408 const Eigen::MatrixBase<Config_t> & ,
409 const Eigen::MatrixBase<Tangent_t> & v,
410 const Eigen::MatrixBase<JacobianIn_t> & Jin,
411 const Eigen::MatrixBase<JacobianOut_t> & J_out)
413 JacobianOut_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, J_out);
414 MotionTpl<Scalar, 0> nu;
415 nu.toVector() << v.template head<2>(), 0, 0, 0, v[2];
417 PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
418 PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED
419 Eigen::Matrix<Scalar, 6, 6> Jtmp6;
421 PINOCCHIO_COMPILER_DIAGNOSTIC_POP
423 Jout.template topRows<2>().noalias() =
424 Jtmp6.template topLeftCorner<2, 2>() * Jin.template topRows<2>();
425 Jout.template topRows<2>().noalias() +=
426 Jtmp6.template topRightCorner<2, 1>() * Jin.template bottomRows<1>();
427 Jout.template bottomRows<1>().noalias() =
428 Jtmp6.template bottomLeftCorner<1, 2>() * Jin.template topRows<2>();
429 Jout.template bottomRows<1>().noalias() +=
430 Jtmp6.template bottomRightCorner<1, 1>() * Jin.template bottomRows<1>();
433 template<
class Config_t,
class Tangent_t,
class Jacobian_t>
434 static void dIntegrateTransport_dq_impl(
435 const Eigen::MatrixBase<Config_t> & ,
436 const Eigen::MatrixBase<Tangent_t> & v,
437 const Eigen::MatrixBase<Jacobian_t> & J)
439 Jacobian_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t, J);
440 PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
441 PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED
445 PINOCCHIO_COMPILER_DIAGNOSTIC_POP
447 Vector2 tinv = (R.transpose() * t).reverse();
448 tinv[0] *= Scalar(-1);
450 Jout.template topRows<2>() = R.transpose() * Jout.template topRows<2>();
452 Jout.template topRows<2>().noalias() += tinv * Jout.template bottomRows<1>();
455 template<
class Config_t,
class Tangent_t,
class Jacobian_t>
456 static void dIntegrateTransport_dv_impl(
457 const Eigen::MatrixBase<Config_t> & ,
458 const Eigen::MatrixBase<Tangent_t> & v,
459 const Eigen::MatrixBase<Jacobian_t> & J)
461 Jacobian_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t, J);
462 MotionTpl<Scalar, 0> nu;
463 nu.toVector() << v.template head<2>(), 0, 0, 0, v[2];
465 PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
466 PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED
467 Eigen::Matrix<Scalar, 6, 6> Jtmp6;
469 PINOCCHIO_COMPILER_DIAGNOSTIC_POP
471 Jout.template topRows<2>() =
472 Jtmp6.template topLeftCorner<2, 2>() * Jout.template topRows<2>();
473 Jout.template topRows<2>().noalias() +=
474 Jtmp6.template topRightCorner<2, 1>() * Jout.template bottomRows<1>();
475 Jout.template bottomRows<1>() =
476 Jtmp6.template bottomRightCorner<1, 1>() * Jout.template bottomRows<1>();
477 Jout.template bottomRows<1>().noalias() +=
478 Jtmp6.template bottomLeftCorner<1, 2>() * Jout.template topRows<2>();
481 template<
class Config_t>
482 static void normalize_impl(
const Eigen::MatrixBase<Config_t> & qout)
487 template<
class Config_t>
488 static bool isNormalized_impl(
const Eigen::MatrixBase<Config_t> & qin,
const Scalar & prec)
490 const Scalar norm = Scalar(qin.template tail<2>().norm());
492 return abs(norm - Scalar(1.0)) < prec;
495 template<
class Config_t>
496 static void random_impl(
const Eigen::MatrixBase<Config_t> & qout)
498 R2crossSO2_t().random(qout);
501 template<
class ConfigL_t,
class ConfigR_t,
class ConfigOut_t>
502 static void randomConfiguration_impl(
503 const Eigen::MatrixBase<ConfigL_t> & lower,
504 const Eigen::MatrixBase<ConfigR_t> & upper,
505 const Eigen::MatrixBase<ConfigOut_t> & qout)
507 R2crossSO2_t().randomConfiguration(lower, upper, qout);
510 template<
class ConfigL_t,
class ConfigR_t>
511 static bool isSameConfiguration_impl(
512 const Eigen::MatrixBase<ConfigL_t> & q0,
513 const Eigen::MatrixBase<ConfigR_t> & q1,
516 return R2crossSO2_t().isSameConfiguration(q0, q1, prec);
520 template<
typename Matrix2Like,
typename Vector2Like,
typename Vector4Like>
522 const Eigen::MatrixBase<Matrix2Like> & R,
523 const Eigen::MatrixBase<Vector2Like> & t,
524 const Eigen::MatrixBase<Vector4Like> & q)
526 EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(Matrix2Like, Matrix2);
527 EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(Vector2Like, Vector2);
528 EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigVector_t, Vector4Like);
530 PINOCCHIO_EIGEN_CONST_CAST(Vector2Like, t) = q.template head<2>();
531 const typename Vector4Like::Scalar &c_theta = q(2), &s_theta = q(3);
532 PINOCCHIO_EIGEN_CONST_CAST(Matrix2Like, R) << c_theta, -s_theta, s_theta, c_theta;
536 template<
typename _Scalar,
int _Options>
539 typedef _Scalar Scalar;
549 template<
typename _Scalar,
int _Options>
551 :
public LieGroupBase<SpecialEuclideanOperationTpl<3, _Scalar, _Options>>
560 typedef Eigen::Quaternion<Scalar, Options> Quaternion_t;
561 typedef Eigen::Map<Quaternion_t> QuaternionMap_t;
562 typedef Eigen::Map<const Quaternion_t> ConstQuaternionMap_t;
565 typedef typename Eigen::NumTraits<Scalar>::Real RealScalar;
581 static ConfigVector_t
neutral()
584 n.template head<6>().setZero();
589 static std::string
name()
591 return std::string(
"SE(3)");
594 template<
class ConfigL_t,
class ConfigR_t,
class Tangent_t>
595 static void difference_impl(
596 const Eigen::MatrixBase<ConfigL_t> & q0,
597 const Eigen::MatrixBase<ConfigR_t> & q1,
598 const Eigen::MatrixBase<Tangent_t> & d)
600 typedef typename Tangent_t::Scalar Scalar;
601 ConstQuaternionMap_t quat0(q0.derived().template tail<4>().data());
603 ConstQuaternionMap_t quat1(q1.derived().template tail<4>().data());
606 typedef Eigen::Matrix<Scalar, 3, 1, PINOCCHIO_EIGEN_PLAIN_TYPE(Tangent_t)::Options> Vector3;
607 const Vector3 dv_pre = q1.derived().template head<3>() - q0.derived().template head<3>();
608 const Vector3 dv = quat0.conjugate() * dv_pre;
609 PINOCCHIO_EIGEN_CONST_CAST(Tangent_t, d).noalias() =
610 log6(quat0.conjugate() * quat1, dv).toVector();
615 template<ArgumentPosition arg,
class ConfigL_t,
class ConfigR_t,
class JacobianOut_t>
617 const Eigen::MatrixBase<ConfigL_t> & q0,
618 const Eigen::MatrixBase<ConfigR_t> & q1,
619 const Eigen::MatrixBase<JacobianOut_t> & J)
621 typedef typename SE3::Vector3 Vector3;
623 ConstQuaternionMap_t quat0(q0.derived().template tail<4>().data());
625 ConstQuaternionMap_t quat1(q1.derived().template tail<4>().data());
628 const Vector3 dv_pre = q1.derived().template head<3>() - q0.derived().template head<3>();
629 const Vector3 trans = quat0.conjugate() * dv_pre;
631 const Quaternion_t quat_diff = quat0.conjugate() * quat1;
633 const SE3 M(quat_diff, trans);
637 PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
638 PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED
641 PINOCCHIO_COMPILER_DIAGNOSTIC_POP
643 const Vector3 p1_p0 = quat1.conjugate() * dv_pre;
645 JacobianOut_t & J0 = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, J);
646 J0.template bottomRightCorner<3, 3>() = J0.template topLeftCorner<3, 3>() =
647 -M.rotation().transpose();
648 J0.template topRightCorner<3, 3>().noalias() =
649 skew(p1_p0) * M.rotation().transpose();
650 J0.template bottomLeftCorner<3, 3>().setZero();
651 J0.applyOnTheLeft(J1);
653 else if (arg == ARG1)
659 template<
class ConfigIn_t,
class Velocity_t,
class ConfigOut_t>
660 static void integrate_impl(
661 const Eigen::MatrixBase<ConfigIn_t> & q,
662 const Eigen::MatrixBase<Velocity_t> & v,
663 const Eigen::MatrixBase<ConfigOut_t> & qout)
665 ConfigOut_t & out = PINOCCHIO_EIGEN_CONST_CAST(ConfigOut_t, qout);
666 Quaternion_t
const quat(q.derived().template tail<4>());
668 QuaternionMap_t res_quat(out.template tail<4>().data());
670 using internal::if_then_else;
672 typedef typename ConfigOut_t::Scalar Scalar;
675 Options = PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigOut_t)::Options
678 Eigen::Matrix<Scalar, 7, 1, Options> expv;
681 out.template head<3>() = (quat * expv.template head<3>()) + q.derived().template head<3>();
683 ConstQuaternionMap_t quat1(expv.template tail<4>().data());
684 res_quat = quat * quat1;
686 const Scalar dot_product = res_quat.dot(quat);
687 for (Eigen::DenseIndex k = 0; k < 4; ++k)
689 res_quat.coeffs().coeffRef(k) = if_then_else(
690 internal::LT, dot_product, Scalar(0),
static_cast<Scalar
>(-res_quat.coeffs().coeff(k)),
691 res_quat.coeffs().coeff(k));
701 template<
class Config_t,
class Jacobian_t>
702 static void integrateCoeffWiseJacobian_impl(
703 const Eigen::MatrixBase<Config_t> & q,
const Eigen::MatrixBase<Jacobian_t> & J)
705 assert(J.rows() ==
nq() && J.cols() ==
nv() &&
"J is not of the right dimension");
707 typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(Config_t) ConfigPlainType;
708 typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(Jacobian_t) JacobianPlainType;
709 typedef typename ConfigPlainType::Scalar Scalar;
711 Jacobian_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t, J);
714 ConstQuaternionMap_t quat_map(q.derived().template tail<4>().data());
716 Jout.template topLeftCorner<3, 3>() = quat_map.toRotationMatrix();
719 typedef Eigen::Matrix<Scalar, 4, 3, JacobianPlainType::Options | Eigen::RowMajor> Jacobian43;
720 typedef SE3Tpl<Scalar, ConfigPlainType::Options> SE3;
721 PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
722 PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED
723 Jacobian43 Jexp3QuatCoeffWise;
728 typename SE3::Matrix3 Jlog;
729 Jlog3(theta, v, Jlog);
730 PINOCCHIO_COMPILER_DIAGNOSTIC_POP
737 if (quat_map.coeffs()[3] >= Scalar(0))
739 PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t, J).template bottomRightCorner<4, 3>().noalias() =
740 Jexp3QuatCoeffWise * Jlog;
742 PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t, J).template bottomRightCorner<4, 3>().noalias() =
743 -Jexp3QuatCoeffWise * Jlog;
746 template<
class Config_t,
class Tangent_t,
class JacobianOut_t>
747 static void dIntegrate_dq_impl(
748 const Eigen::MatrixBase<Config_t> & ,
749 const Eigen::MatrixBase<Tangent_t> & v,
750 const Eigen::MatrixBase<JacobianOut_t> & J,
751 const AssignmentOperatorType op = SETTO)
753 JacobianOut_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, J);
758 Jout =
exp6(MotionRef<const Tangent_t>(v.derived())).toDualActionMatrix().transpose();
761 Jout +=
exp6(MotionRef<const Tangent_t>(v.derived())).toDualActionMatrix().transpose();
764 Jout -=
exp6(MotionRef<const Tangent_t>(v.derived())).toDualActionMatrix().transpose();
767 assert(
false &&
"Wrong Op requesed value");
772 template<
class Config_t,
class Tangent_t,
class JacobianOut_t>
773 static void dIntegrate_dv_impl(
774 const Eigen::MatrixBase<Config_t> & ,
775 const Eigen::MatrixBase<Tangent_t> & v,
776 const Eigen::MatrixBase<JacobianOut_t> & J,
777 const AssignmentOperatorType op = SETTO)
782 Jexp6<SETTO>(MotionRef<const Tangent_t>(v.derived()), J.derived());
785 Jexp6<ADDTO>(MotionRef<const Tangent_t>(v.derived()), J.derived());
788 Jexp6<RMTO>(MotionRef<const Tangent_t>(v.derived()), J.derived());
791 assert(
false &&
"Wrong Op requesed value");
796 template<
class Config_t,
class Tangent_t,
class JacobianIn_t,
class JacobianOut_t>
797 static void dIntegrateTransport_dq_impl(
798 const Eigen::MatrixBase<Config_t> & ,
799 const Eigen::MatrixBase<Tangent_t> & v,
800 const Eigen::MatrixBase<JacobianIn_t> & Jin,
801 const Eigen::MatrixBase<JacobianOut_t> & J_out)
803 JacobianOut_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, J_out);
804 Eigen::Matrix<Scalar, 6, 6> Jtmp6;
805 Jtmp6 =
exp6(MotionRef<const Tangent_t>(v.derived())).toDualActionMatrix().transpose();
807 Jout.template topRows<3>().noalias() =
808 Jtmp6.template topLeftCorner<3, 3>() * Jin.template topRows<3>();
809 Jout.template topRows<3>().noalias() +=
810 Jtmp6.template topRightCorner<3, 3>() * Jin.template bottomRows<3>();
811 Jout.template bottomRows<3>().noalias() =
812 Jtmp6.template bottomRightCorner<3, 3>() * Jin.template bottomRows<3>();
815 template<
class Config_t,
class Tangent_t,
class JacobianIn_t,
class JacobianOut_t>
816 static void dIntegrateTransport_dv_impl(
817 const Eigen::MatrixBase<Config_t> & ,
818 const Eigen::MatrixBase<Tangent_t> & v,
819 const Eigen::MatrixBase<JacobianIn_t> & Jin,
820 const Eigen::MatrixBase<JacobianOut_t> & J_out)
822 JacobianOut_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, J_out);
823 PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
824 PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED
825 Eigen::Matrix<Scalar, 6, 6> Jtmp6;
826 Jexp6<SETTO>(MotionRef<const Tangent_t>(v.derived()), Jtmp6);
827 PINOCCHIO_COMPILER_DIAGNOSTIC_POP
829 Jout.template topRows<3>().noalias() =
830 Jtmp6.template topLeftCorner<3, 3>() * Jin.template topRows<3>();
831 Jout.template topRows<3>().noalias() +=
832 Jtmp6.template topRightCorner<3, 3>() * Jin.template bottomRows<3>();
833 Jout.template bottomRows<3>().noalias() =
834 Jtmp6.template bottomRightCorner<3, 3>() * Jin.template bottomRows<3>();
837 template<
class Config_t,
class Tangent_t,
class Jacobian_t>
838 static void dIntegrateTransport_dq_impl(
839 const Eigen::MatrixBase<Config_t> & ,
840 const Eigen::MatrixBase<Tangent_t> & v,
841 const Eigen::MatrixBase<Jacobian_t> & J_out)
843 Jacobian_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t, J_out);
844 Eigen::Matrix<Scalar, 6, 6> Jtmp6;
845 Jtmp6 =
exp6(MotionRef<const Tangent_t>(v.derived())).toDualActionMatrix().transpose();
848 Jout.template topRows<3>() =
849 Jtmp6.template topLeftCorner<3, 3>() * Jout.template topRows<3>();
850 Jout.template topRows<3>().noalias() +=
851 Jtmp6.template topRightCorner<3, 3>() * Jout.template bottomRows<3>();
852 Jout.template bottomRows<3>() =
853 Jtmp6.template bottomRightCorner<3, 3>() * Jout.template bottomRows<3>();
856 template<
class Config_t,
class Tangent_t,
class Jacobian_t>
857 static void dIntegrateTransport_dv_impl(
858 const Eigen::MatrixBase<Config_t> & ,
859 const Eigen::MatrixBase<Tangent_t> & v,
860 const Eigen::MatrixBase<Jacobian_t> & J_out)
862 Jacobian_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t, J_out);
863 PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
864 PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED
865 Eigen::Matrix<Scalar, 6, 6> Jtmp6;
866 Jexp6<SETTO>(MotionRef<const Tangent_t>(v.derived()), Jtmp6);
867 PINOCCHIO_COMPILER_DIAGNOSTIC_POP
869 Jout.template topRows<3>() =
870 Jtmp6.template topLeftCorner<3, 3>() * Jout.template topRows<3>();
871 Jout.template topRows<3>().noalias() +=
872 Jtmp6.template topRightCorner<3, 3>() * Jout.template bottomRows<3>();
873 Jout.template bottomRows<3>() =
874 Jtmp6.template bottomRightCorner<3, 3>() * Jout.template bottomRows<3>();
877 template<
class ConfigL_t,
class ConfigR_t>
878 static Scalar squaredDistance_impl(
879 const Eigen::MatrixBase<ConfigL_t> & q0,
const Eigen::MatrixBase<ConfigR_t> & q1)
881 PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
882 PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED
884 difference_impl(q0, q1, t);
885 PINOCCHIO_COMPILER_DIAGNOSTIC_POP
886 return t.squaredNorm();
889 template<
class Config_t>
890 static void normalize_impl(
const Eigen::MatrixBase<Config_t> & qout)
895 template<
class Config_t>
896 static bool isNormalized_impl(
const Eigen::MatrixBase<Config_t> & qin,
const Scalar & prec)
898 Scalar norm = Scalar(qin.template tail<4>().norm());
900 return abs(norm - Scalar(1.0)) < prec;
903 template<
class Config_t>
904 static void random_impl(
const Eigen::MatrixBase<Config_t> & qout)
906 R3crossSO3_t().random(qout);
909 template<
class ConfigL_t,
class ConfigR_t,
class ConfigOut_t>
910 static void randomConfiguration_impl(
911 const Eigen::MatrixBase<ConfigL_t> & lower,
912 const Eigen::MatrixBase<ConfigR_t> & upper,
913 const Eigen::MatrixBase<ConfigOut_t> & qout)
915 R3crossSO3_t().randomConfiguration(lower, upper, qout);
918 template<
class ConfigL_t,
class ConfigR_t>
919 static bool isSameConfiguration_impl(
920 const Eigen::MatrixBase<ConfigL_t> & q0,
921 const Eigen::MatrixBase<ConfigR_t> & q1,
924 return R3crossSO3_t().isSameConfiguration(q0, q1, prec);
void firstOrderNormalize(const Eigen::QuaternionBase< D > &q)
void exp6(const MotionDense< MotionDerived > &motion, Eigen::MatrixBase< Config_t > &qout)
The se3 -> SE3 exponential map, using quaternions to represent the output rotation.
bool isNormalized(const Eigen::QuaternionBase< Quaternion > &quat, const typename Quaternion::Coefficients::RealScalar &prec)
Check whether the input quaternion is Normalized within the given precision.
Eigen::Matrix< typename QuaternionLike::Scalar, 3, 1, typename QuaternionLike::Vector3 ::Options > log3(const Eigen::QuaternionBase< QuaternionLike > &quat, typename QuaternionLike::Scalar &theta)
Same as log3 but with a unit quaternion as input.
void Jexp3CoeffWise(const Eigen::MatrixBase< Vector3Like > &v, const Eigen::MatrixBase< Matrix43Like > &Jexp)
Derivative of where is a small perturbation of at identity.
Main pinocchio namespace.
void Jexp6(const MotionDense< MotionDerived > &nu, const Eigen::MatrixBase< Matrix6Like > &Jexp)
Derivative of exp6 Computed as the inverse of Jlog6.
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 Jlog3(const Scalar &theta, const Eigen::MatrixBase< Vector3Like > &log, const Eigen::MatrixBase< Matrix3Like > &Jlog)
Derivative of log3.
void forwardKinematics(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q)
Update the joint placements according to the current joint configuration.
int nv(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointNvVisitor to get the dimension of the joint tangent space.
int nq(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointNqVisitor to get the dimension of the joint configuration space.
void Jlog6(const SE3Tpl< Scalar, Options > &M, const Eigen::MatrixBase< Matrix6Like > &Jlog)
Derivative of log6.
std::string name(const LieGroupGenericTpl< LieGroupCollection > &lg)
Visit a LieGroupVariant to get the name of it.
void normalize(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorType > &qout)
Normalize a configuration vector.
void neutral(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ReturnType > &qout)
Return the neutral configuration element related to the model configuration space.
void SINCOS(const S1 &a, S2 *sa, S3 *ca)
Computes sin/cos values of a given input scalar.
void skew(const Eigen::MatrixBase< Vector3 > &v, const Eigen::MatrixBase< Matrix3 > &M)
Computes the skew representation of a given 3d vector, i.e. the antisymmetric matrix representation o...
static Index nv()
Get dimension of Lie Group tangent space.
static Index nv()
Get dimension of Lie Group tangent space.
static void dDifference_impl(const Eigen::MatrixBase< ConfigL_t > &q0, const Eigen::MatrixBase< ConfigR_t > &q1, const Eigen::MatrixBase< JacobianOut_t > &J)
Common traits structure to fully define base classes for CRTP.