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/spatial/fwd.hpp"
13 #include "pinocchio/utils/static-if.hpp"
14 #include "pinocchio/spatial/se3.hpp"
15 #include "pinocchio/multibody/liegroup/liegroup-base.hpp"
17 #include "pinocchio/multibody/liegroup/vector-space.hpp"
18 #include "pinocchio/multibody/liegroup/cartesian-product.hpp"
19 #include "pinocchio/multibody/liegroup/special-orthogonal.hpp"
23 template<
int Dim,
typename Scalar,
int Options = 0>
27 template<
int Dim,
typename Scalar,
int Options>
31 template<
typename _Scalar,
int _Options>
34 typedef _Scalar Scalar;
44 template<
typename _Scalar,
int _Options>
46 :
public LieGroupBase <SpecialEuclideanOperationTpl<2,_Scalar,_Options> >
54 typedef Eigen::Matrix<Scalar,2,2,Options> Matrix2;
55 typedef Eigen::Matrix<Scalar,2,1,Options> Vector2;
57 template<
typename TangentVector,
typename Matrix2Like,
typename Vector2Like>
58 static void exp(
const Eigen::MatrixBase<TangentVector> & v,
59 const Eigen::MatrixBase<Matrix2Like> & R,
60 const Eigen::MatrixBase<Vector2Like> & t)
62 EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(TangentVector_t,TangentVector);
63 EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix2Like, 2, 2);
64 EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector2Like, 2);
66 typedef typename Matrix2Like::Scalar Scalar;
67 const Scalar omega = v(2);
68 Scalar cv,sv;
SINCOS(omega, &sv, &cv);
69 PINOCCHIO_EIGEN_CONST_CAST(Matrix2Like,R) << cv, -sv, sv, cv;
70 using internal::if_then_else;
73 typename PINOCCHIO_EIGEN_PLAIN_TYPE(Vector2Like) vcross(-v(1), v(0));
74 vcross -= -v(1)*R.col(0) + v(0)*R.col(1);
76 Scalar omega_abs = math::fabs(omega);
77 PINOCCHIO_EIGEN_CONST_CAST(Vector2Like,t).coeffRef(0) = if_then_else(internal::GT, omega_abs , Scalar(1e-14),
81 PINOCCHIO_EIGEN_CONST_CAST(Vector2Like,t).coeffRef(1) = if_then_else(internal::GT, omega_abs, Scalar(1e-14),
88 template<
typename Matrix2Like,
typename Vector2Like,
typename Matrix3Like>
89 static void toInverseActionMatrix(
const Eigen::MatrixBase<Matrix2Like> & R,
90 const Eigen::MatrixBase<Vector2Like> & t,
91 const Eigen::MatrixBase<Matrix3Like> & M,
92 const AssignmentOperatorType op)
94 EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix2Like, 2, 2);
95 EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector2Like, 2);
96 EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix3Like, 3, 3);
97 Matrix3Like & Mout = PINOCCHIO_EIGEN_CONST_CAST(Matrix3Like,M);
98 typedef typename Matrix3Like::Scalar Scalar;
100 typename PINOCCHIO_EIGEN_PLAIN_TYPE(Vector2Like) tinv((R.transpose() * t).reverse());
101 tinv[0] *= Scalar(-1.);
105 Mout.template topLeftCorner<2,2>() = R.transpose();
106 Mout.template topRightCorner<2,1>() = tinv;
107 Mout.template bottomLeftCorner<1,2>().setZero();
108 Mout(2,2) = (Scalar)1;
111 Mout.template topLeftCorner<2,2>() += R.transpose();
112 Mout.template topRightCorner<2,1>() += tinv;
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 assert(
false &&
"Wrong Op requesed value");
129 template<
typename Matrix2Like,
typename Vector2Like,
typename TangentVector>
130 static void log(
const Eigen::MatrixBase<Matrix2Like> & R,
131 const Eigen::MatrixBase<Vector2Like> & p,
132 const Eigen::MatrixBase<TangentVector> & v)
134 EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix2Like, 2, 2);
135 EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector2Like, 2);
136 EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(TangentVector_t,TangentVector);
138 TangentVector & vout = PINOCCHIO_EIGEN_CONST_CAST(TangentVector,v);
140 typedef typename Matrix2Like::Scalar Scalar1;
142 Scalar1 t = SO2_t::log(R);
143 const Scalar1 tabs = math::fabs(t);
144 const Scalar1 t2 = t*t;
145 Scalar1 st,ct;
SINCOS(tabs, &st, &ct);
147 alpha = internal::if_then_else(internal::LT, tabs, Scalar(1e-4),
148 1 - t2/12 - t2*t2/720,
151 vout.template head<2>().noalias() = alpha * p;
152 vout(0) += t/2 * p(1);
153 vout(1) += -t/2 * p(0);
157 template<
typename Matrix2Like,
typename Vector2Like,
typename JacobianOutLike>
158 static void Jlog(
const Eigen::MatrixBase<Matrix2Like> & R,
159 const Eigen::MatrixBase<Vector2Like> & p,
160 const Eigen::MatrixBase<JacobianOutLike> & J)
162 EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix2Like, 2, 2);
163 EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector2Like, 2);
164 EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(JacobianOutLike, JacobianMatrix_t);
166 JacobianOutLike & Jout = PINOCCHIO_EIGEN_CONST_CAST(JacobianOutLike,J);
168 typedef typename Matrix2Like::Scalar Scalar1;
170 Scalar1 t = SO2_t::log(R);
171 const Scalar1 tabs = math::fabs(t);
172 Scalar1 alpha, alpha_dot;
174 Scalar1 st,ct;
SINCOS(t, &st, &ct);
175 Scalar1 inv_2_1_ct = 0.5 / (1-ct);
177 alpha = internal::if_then_else(internal::LT, tabs, Scalar(1e-4),
180 alpha_dot = internal::if_then_else(internal::LT, tabs, Scalar(1e-4),
181 - t / 6 - t2*t / 180,
182 (st-t) * inv_2_1_ct);
184 typename PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix2Like) V;
185 V(0,0) = V(1,1) = alpha;
189 Jout.template topLeftCorner <2,2>().noalias() = V * R;
190 Jout.template topRightCorner<2,1>() << alpha_dot*p[0] + p[1]/2, -p(0)/2 + alpha_dot*p(1);
191 Jout.template bottomLeftCorner<1,2>().setZero();
209 static ConfigVector_t
neutral()
211 ConfigVector_t n; n << Scalar(0), Scalar(0), Scalar(1), Scalar(0);
215 static std::string
name()
217 return std::string(
"SE(2)");
220 template <
class ConfigL_t,
class ConfigR_t,
class Tangent_t>
221 static void difference_impl(
const Eigen::MatrixBase<ConfigL_t> & q0,
222 const Eigen::MatrixBase<ConfigR_t> & q1,
223 const Eigen::MatrixBase<Tangent_t> & d)
225 Matrix2 R0, R1; Vector2 t0, t1;
228 Matrix2 R (R0.transpose() * R1);
229 Vector2 t (R0.transpose() * (t1 - t0));
234 template <ArgumentPosition arg,
class ConfigL_t,
class ConfigR_t,
class JacobianOut_t>
235 void dDifference_impl(
const Eigen::MatrixBase<ConfigL_t> & q0,
236 const Eigen::MatrixBase<ConfigR_t> & q1,
237 const Eigen::MatrixBase<JacobianOut_t>& J)
const
239 Matrix2 R0, R1; Vector2 t0, t1;
242 Matrix2 R (R0.transpose() * R1);
243 Vector2 t (R0.transpose() * (t1 - t0));
250 Vector2 pcross (q1(1) - q0(1), q0(0) - q1(0));
252 JacobianOut_t& J0 = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, J);
253 J0.template topLeftCorner <2,2> ().noalias() = - R.transpose();
254 J0.template topRightCorner<2,1> ().noalias() = R1.transpose() * pcross;
255 J0.template bottomLeftCorner <1,2> ().setZero();
257 J0.applyOnTheLeft(J1);
258 }
else if (arg == ARG1) {
263 template <
class ConfigIn_t,
class Velocity_t,
class ConfigOut_t>
264 static void integrate_impl(
const Eigen::MatrixBase<ConfigIn_t> & q,
265 const Eigen::MatrixBase<Velocity_t> & v,
266 const Eigen::MatrixBase<ConfigOut_t> & qout)
268 ConfigOut_t & out = PINOCCHIO_EIGEN_CONST_CAST(ConfigOut_t,qout);
275 out.template head<2>().noalias() = R0 * t + t0;
276 out.template tail<2>().noalias() = R0 * R.col(0);
279 template <
class Config_t,
class Jacobian_t>
280 static void integrateCoeffWiseJacobian_impl(
const Eigen::MatrixBase<Config_t> & q,
281 const Eigen::MatrixBase<Jacobian_t> & J)
283 assert(J.rows() ==
nq() && J.cols() ==
nv() &&
"J is not of the right dimension");
285 Jacobian_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t,J);
288 const typename Config_t::Scalar & c_theta = q(2),
291 Jout.template topLeftCorner<2,2>() << c_theta, -s_theta, s_theta, c_theta;
292 Jout.template bottomRightCorner<2,1>() << -s_theta, c_theta;
295 template <
class Config_t,
class Tangent_t,
class JacobianOut_t>
296 static void dIntegrate_dq_impl(
const Eigen::MatrixBase<Config_t > & ,
297 const Eigen::MatrixBase<Tangent_t> & v,
298 const Eigen::MatrixBase<JacobianOut_t>& J,
299 const AssignmentOperatorType op=SETTO)
301 JacobianOut_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J);
307 toInverseActionMatrix(R, t, Jout, op);
310 template <
class Config_t,
class Tangent_t,
class JacobianOut_t>
311 static void dIntegrate_dv_impl(
const Eigen::MatrixBase<Config_t > & ,
312 const Eigen::MatrixBase<Tangent_t> & v,
313 const Eigen::MatrixBase<JacobianOut_t> & J,
314 const AssignmentOperatorType op=SETTO)
316 JacobianOut_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J);
318 MotionTpl<Scalar,0> nu; nu.toVector() << v.template head<2>(), 0, 0, 0, v[2];
319 Eigen::Matrix<Scalar,6,6> Jtmp6;
325 Jout << Jtmp6.template topLeftCorner<2,2>(), Jtmp6.template topRightCorner<2,1>(),
326 Jtmp6.template bottomLeftCorner<1,2>(), Jtmp6.template bottomRightCorner<1,1>();
329 Jout.template topLeftCorner<2,2>() += Jtmp6.template topLeftCorner<2,2>();
330 Jout.template topRightCorner<2,1>() += Jtmp6.template topRightCorner<2,1>();
331 Jout.template bottomLeftCorner<1,2>() += Jtmp6.template bottomLeftCorner<1,2>();
332 Jout.template bottomRightCorner<1,1>() += Jtmp6.template bottomRightCorner<1,1>();
335 Jout.template topLeftCorner<2,2>() -= Jtmp6.template topLeftCorner<2,2>();
336 Jout.template topRightCorner<2,1>() -= Jtmp6.template topRightCorner<2,1>();
337 Jout.template bottomLeftCorner<1,2>() -= Jtmp6.template bottomLeftCorner<1,2>();
338 Jout.template bottomRightCorner<1,1>() -= Jtmp6.template bottomRightCorner<1,1>();
341 assert(
false &&
"Wrong Op requesed value");
346 template <
class Config_t,
class Tangent_t,
class JacobianIn_t,
class JacobianOut_t>
347 void dIntegrateTransport_dq_impl(
const Eigen::MatrixBase<Config_t > & ,
348 const Eigen::MatrixBase<Tangent_t> & v,
349 const Eigen::MatrixBase<JacobianIn_t> & Jin,
350 const Eigen::MatrixBase<JacobianOut_t> & J_out)
const
352 JacobianOut_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J_out);
357 Vector2 tinv = (R.transpose() * t).reverse();
358 tinv[0] *= Scalar(-1.);
360 Jout.template topRows<2>().noalias() = R.transpose() * Jin.template topRows<2>();
361 Jout.template topRows<2>().noalias() += tinv * Jin.template bottomRows<1>();
362 Jout.template bottomRows<1>() = Jin.template bottomRows<1>();
365 template <
class Config_t,
class Tangent_t,
class JacobianIn_t,
class JacobianOut_t>
366 void dIntegrateTransport_dv_impl(
const Eigen::MatrixBase<Config_t > & ,
367 const Eigen::MatrixBase<Tangent_t> & v,
368 const Eigen::MatrixBase<JacobianIn_t> & Jin,
369 const Eigen::MatrixBase<JacobianOut_t> & J_out)
const
371 JacobianOut_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J_out);
372 MotionTpl<Scalar,0> nu; nu.toVector() << v.template head<2>(), 0, 0, 0, v[2];
374 Eigen::Matrix<Scalar,6,6> Jtmp6;
377 Jout.template topRows<2>().noalias()
378 = Jtmp6.template topLeftCorner<2,2>() * Jin.template topRows<2>();
379 Jout.template topRows<2>().noalias()
380 += Jtmp6.template topRightCorner<2,1>() * Jin.template bottomRows<1>();
381 Jout.template bottomRows<1>().noalias()
382 = Jtmp6.template bottomLeftCorner<1,2>()* Jin.template topRows<2>();
383 Jout.template bottomRows<1>().noalias()
384 += Jtmp6.template bottomRightCorner<1,1>() * Jin.template bottomRows<1>();
388 template <
class Config_t,
class Tangent_t,
class Jacobian_t>
389 void dIntegrateTransport_dq_impl(
const Eigen::MatrixBase<Config_t > & ,
390 const Eigen::MatrixBase<Tangent_t> & v,
391 const Eigen::MatrixBase<Jacobian_t> & J)
const
393 Jacobian_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t,J);
398 Vector2 tinv = (R.transpose() * t).reverse();
399 tinv[0] *= Scalar(-1);
401 Jout.template topRows<2>() = R.transpose() * Jout.template topRows<2>();
403 Jout.template topRows<2>().noalias() += tinv * Jout.template bottomRows<1>();
406 template <
class Config_t,
class Tangent_t,
class Jacobian_t>
407 void dIntegrateTransport_dv_impl(
const Eigen::MatrixBase<Config_t > & ,
408 const Eigen::MatrixBase<Tangent_t> & v,
409 const Eigen::MatrixBase<Jacobian_t> & J)
const
411 Jacobian_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t,J);
412 MotionTpl<Scalar,0> nu; nu.toVector() << v.template head<2>(), 0, 0, 0, v[2];
414 Eigen::Matrix<Scalar,6,6> Jtmp6;
417 Jout.template topRows<2>()
418 = Jtmp6.template topLeftCorner<2,2>() * Jout.template topRows<2>();
419 Jout.template topRows<2>().noalias()
420 += Jtmp6.template topRightCorner<2,1>() * Jout.template bottomRows<1>();
421 Jout.template bottomRows<1>()
422 = Jtmp6.template bottomRightCorner<1,1>() * Jout.template bottomRows<1>();
423 Jout.template bottomRows<1>().noalias()
424 += Jtmp6.template bottomLeftCorner<1,2>() * Jout.template topRows<2>();
427 template <
class Config_t>
428 static void normalize_impl(
const Eigen::MatrixBase<Config_t> & qout)
430 PINOCCHIO_EIGEN_CONST_CAST(Config_t,qout).template tail<2>().normalize();
433 template <
class Config_t>
434 void random_impl(
const Eigen::MatrixBase<Config_t> & qout)
const
436 R2crossSO2_t().random(qout);
439 template <
class ConfigL_t,
class ConfigR_t,
class ConfigOut_t>
440 void randomConfiguration_impl(
const Eigen::MatrixBase<ConfigL_t> & lower,
441 const Eigen::MatrixBase<ConfigR_t> & upper,
442 const Eigen::MatrixBase<ConfigOut_t> & qout)
445 R2crossSO2_t ().randomConfiguration(lower, upper, qout);
448 template <
class ConfigL_t,
class ConfigR_t>
449 static bool isSameConfiguration_impl(
const Eigen::MatrixBase<ConfigL_t> & q0,
450 const Eigen::MatrixBase<ConfigR_t> & q1,
453 return R2crossSO2_t().isSameConfiguration(q0, q1, prec);
458 template<
typename Matrix2Like,
typename Vector2Like,
typename Vector4Like>
460 const Eigen::MatrixBase<Vector2Like> & t,
461 const Eigen::MatrixBase<Vector4Like> & q)
463 EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(Matrix2Like, Matrix2);
464 EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(Vector2Like, Vector2);
465 EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigVector_t,Vector4Like);
467 PINOCCHIO_EIGEN_CONST_CAST(Vector2Like,t) = q.template head<2>();
468 const typename Vector4Like::Scalar & c_theta = q(2),
470 PINOCCHIO_EIGEN_CONST_CAST(Matrix2Like,R) << c_theta, -s_theta, s_theta, c_theta;
475 template<
typename _Scalar,
int _Options>
478 typedef _Scalar Scalar;
488 template<
typename _Scalar,
int _Options>
490 :
public LieGroupBase <SpecialEuclideanOperationTpl<3,_Scalar,_Options> >
496 typedef Eigen::Quaternion<Scalar,Options> Quaternion_t;
497 typedef Eigen::Map< Quaternion_t> QuaternionMap_t;
498 typedef Eigen::Map<const Quaternion_t> ConstQuaternionMap_t;
501 typedef typename Eigen::NumTraits<Scalar>::Real RealScalar;
517 static ConfigVector_t
neutral()
519 ConfigVector_t n; n.template head<6>().setZero(); n[6] = 1;
523 static std::string
name()
525 return std::string(
"SE(3)");
528 template <
class ConfigL_t,
class ConfigR_t,
class Tangent_t>
529 static void difference_impl(
const Eigen::MatrixBase<ConfigL_t> & q0,
530 const Eigen::MatrixBase<ConfigR_t> & q1,
531 const Eigen::MatrixBase<Tangent_t> & d)
533 ConstQuaternionMap_t quat0 (q0.derived().template tail<4>().data());
535 ConstQuaternionMap_t quat1 (q1.derived().template tail<4>().data());
538 PINOCCHIO_EIGEN_CONST_CAST(Tangent_t,d)
539 =
log6( SE3(quat0.matrix(), q0.derived().template head<3>()).
inverse()
540 * SE3(quat1.matrix(), q1.derived().template head<3>())).toVector();
544 template <ArgumentPosition arg,
class ConfigL_t,
class ConfigR_t,
class JacobianOut_t>
546 const Eigen::MatrixBase<ConfigR_t> & q1,
547 const Eigen::MatrixBase<JacobianOut_t> & J)
const
549 typedef typename SE3::Vector3 Vector3;
550 typedef typename SE3::Matrix3 Matrix3;
552 ConstQuaternionMap_t quat0 (q0.derived().template tail<4>().data());
554 ConstQuaternionMap_t quat1 (q1.derived().template tail<4>().data());
557 Matrix3 R0(quat0.matrix()), R1 (quat1.matrix());
561 *
SE3(R1, q1.template head<3>()));
567 const Vector3 p1_p0 = R1.transpose()*(q1.template head<3>() - q0.template head<3>());
569 JacobianOut_t & J0 = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J);
570 J0.template bottomRightCorner<3,3> ().noalias() = J0.template topLeftCorner <3,3> ().noalias() = - M.rotation().transpose();
571 J0.template topRightCorner<3,3> ().noalias() =
skew(p1_p0) * M.rotation().transpose();
572 J0.template bottomLeftCorner<3,3> ().setZero();
573 J0.applyOnTheLeft(J1);
575 else if (arg == ARG1) {
580 template <
class ConfigIn_t,
class Velocity_t,
class ConfigOut_t>
581 static void integrate_impl(
const Eigen::MatrixBase<ConfigIn_t> & q,
582 const Eigen::MatrixBase<Velocity_t> & v,
583 const Eigen::MatrixBase<ConfigOut_t> & qout)
585 ConfigOut_t & out = PINOCCHIO_EIGEN_CONST_CAST(ConfigOut_t,qout);
586 ConstQuaternionMap_t quat(q.derived().template tail<4>().data());
588 QuaternionMap_t res_quat (out.template tail<4>().data());
590 using internal::if_then_else;
592 SE3 M0 (quat.matrix(), q.derived().template head<3>());
596 out.template head<3>() = M1.translation();
597 quaternion::assignQuaternion(res_quat,M1.rotation());
598 const Scalar dot_product = res_quat.dot(quat);
599 for(Eigen::DenseIndex k = 0; k < 4; ++k)
601 res_quat.coeffs().coeffRef(k) = if_then_else(internal::LT, dot_product, Scalar(0),
602 -res_quat.coeffs().coeff(k),
603 res_quat.coeffs().coeff(k));
612 template <
class Config_t,
class Jacobian_t>
613 static void integrateCoeffWiseJacobian_impl(
const Eigen::MatrixBase<Config_t> & q,
614 const Eigen::MatrixBase<Jacobian_t> & J)
616 assert(J.rows() ==
nq() && J.cols() ==
nv() &&
"J is not of the right dimension");
618 typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(Config_t) ConfigPlainType;
619 typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(Jacobian_t) JacobianPlainType;
620 typedef typename ConfigPlainType::Scalar Scalar;
622 Jacobian_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t,J);
625 ConstQuaternionMap_t quat_map(q.derived().template tail<4>().data());
627 Jout.template topLeftCorner<3,3>() = quat_map.toRotationMatrix();
630 typedef Eigen::Matrix<Scalar,4,3,JacobianPlainType::Options|Eigen::RowMajor> Jacobian43;
631 typedef SE3Tpl<Scalar,ConfigPlainType::Options> SE3;
632 Jacobian43 Jexp3QuatCoeffWise;
637 typename SE3::Matrix3 Jlog;
644 if(quat_map.coeffs()[3] >= Scalar(0))
645 PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t,J).template bottomRightCorner<4,3>().noalias() = Jexp3QuatCoeffWise * Jlog;
647 PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t,J).template bottomRightCorner<4,3>().noalias() = -Jexp3QuatCoeffWise * Jlog;
650 template <
class Config_t,
class Tangent_t,
class JacobianOut_t>
651 static void dIntegrate_dq_impl(
const Eigen::MatrixBase<Config_t > & ,
652 const Eigen::MatrixBase<Tangent_t> & v,
653 const Eigen::MatrixBase<JacobianOut_t>& J,
654 const AssignmentOperatorType op=SETTO)
656 JacobianOut_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J);
661 Jout =
exp6(MotionRef<const Tangent_t>(v.derived())).toDualActionMatrix().transpose();
664 Jout +=
exp6(MotionRef<const Tangent_t>(v.derived())).toDualActionMatrix().transpose();
667 Jout -=
exp6(MotionRef<const Tangent_t>(v.derived())).toDualActionMatrix().transpose();
670 assert(
false &&
"Wrong Op requesed value");
675 template <
class Config_t,
class Tangent_t,
class JacobianOut_t>
676 static void dIntegrate_dv_impl(
const Eigen::MatrixBase<Config_t > & ,
677 const Eigen::MatrixBase<Tangent_t> & v,
678 const Eigen::MatrixBase<JacobianOut_t>& J,
679 const AssignmentOperatorType op=SETTO)
684 Jexp6<SETTO>(MotionRef<const Tangent_t>(v.derived()), J.derived());
687 Jexp6<ADDTO>(MotionRef<const Tangent_t>(v.derived()), J.derived());
690 Jexp6<RMTO>(MotionRef<const Tangent_t>(v.derived()), J.derived());
693 assert(
false &&
"Wrong Op requesed value");
698 template <
class Config_t,
class Tangent_t,
class JacobianIn_t,
class JacobianOut_t>
699 void dIntegrateTransport_dq_impl(
const Eigen::MatrixBase<Config_t > & ,
700 const Eigen::MatrixBase<Tangent_t> & v,
701 const Eigen::MatrixBase<JacobianIn_t> & Jin,
702 const Eigen::MatrixBase<JacobianOut_t> & J_out)
const
704 JacobianOut_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J_out);
705 Eigen::Matrix<Scalar,6,6> Jtmp6;
706 Jtmp6 =
exp6(MotionRef<const Tangent_t>(v.derived())).toDualActionMatrix().transpose();
708 Jout.template topRows<3>().noalias()
709 = Jtmp6.template topLeftCorner<3,3>() * Jin.template topRows<3>();
710 Jout.template topRows<3>().noalias()
711 += Jtmp6.template topRightCorner<3,3>() * Jin.template bottomRows<3>();
712 Jout.template bottomRows<3>().noalias()
713 = Jtmp6.template bottomRightCorner<3,3>() * Jin.template bottomRows<3>();
716 template <
class Config_t,
class Tangent_t,
class JacobianIn_t,
class JacobianOut_t>
717 void dIntegrateTransport_dv_impl(
const Eigen::MatrixBase<Config_t > & ,
718 const Eigen::MatrixBase<Tangent_t> & v,
719 const Eigen::MatrixBase<JacobianIn_t> & Jin,
720 const Eigen::MatrixBase<JacobianOut_t> & J_out)
const
722 JacobianOut_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J_out);
723 Eigen::Matrix<Scalar,6,6> Jtmp6;
724 Jexp6<SETTO>(MotionRef<const Tangent_t>(v.derived()), Jtmp6);
726 Jout.template topRows<3>().noalias()
727 = Jtmp6.template topLeftCorner<3,3>() * Jin.template topRows<3>();
728 Jout.template topRows<3>().noalias()
729 += Jtmp6.template topRightCorner<3,3>() * Jin.template bottomRows<3>();
730 Jout.template bottomRows<3>().noalias()
731 = Jtmp6.template bottomRightCorner<3,3>() * Jin.template bottomRows<3>();
735 template <
class Config_t,
class Tangent_t,
class Jacobian_t>
736 void dIntegrateTransport_dq_impl(
const Eigen::MatrixBase<Config_t > & ,
737 const Eigen::MatrixBase<Tangent_t> & v,
738 const Eigen::MatrixBase<Jacobian_t> & J_out)
const
740 Jacobian_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t,J_out);
741 Eigen::Matrix<Scalar,6,6> Jtmp6;
742 Jtmp6 =
exp6(MotionRef<const Tangent_t>(v.derived())).toDualActionMatrix().transpose();
745 Jout.template topRows<3>()
746 = Jtmp6.template topLeftCorner<3,3>() * Jout.template topRows<3>();
747 Jout.template topRows<3>().noalias()
748 += Jtmp6.template topRightCorner<3,3>() * Jout.template bottomRows<3>();
749 Jout.template bottomRows<3>()
750 = Jtmp6.template bottomRightCorner<3,3>() * Jout.template bottomRows<3>();
753 template <
class Config_t,
class Tangent_t,
class Jacobian_t>
754 void dIntegrateTransport_dv_impl(
const Eigen::MatrixBase<Config_t > & ,
755 const Eigen::MatrixBase<Tangent_t> & v,
756 const Eigen::MatrixBase<Jacobian_t> & J_out)
const
758 Jacobian_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t,J_out);
759 Eigen::Matrix<Scalar,6,6> Jtmp6;
760 Jexp6<SETTO>(MotionRef<const Tangent_t>(v.derived()), Jtmp6);
762 Jout.template topRows<3>()
763 = Jtmp6.template topLeftCorner<3,3>() * Jout.template topRows<3>();
764 Jout.template topRows<3>().noalias()
765 += Jtmp6.template topRightCorner<3,3>() * Jout.template bottomRows<3>();
766 Jout.template bottomRows<3>()
767 = Jtmp6.template bottomRightCorner<3,3>() * Jout.template bottomRows<3>();
770 template <
class ConfigL_t,
class ConfigR_t>
771 static Scalar squaredDistance_impl(
const Eigen::MatrixBase<ConfigL_t> & q0,
772 const Eigen::MatrixBase<ConfigR_t> & q1)
775 difference_impl(q0, q1, t);
776 return t.squaredNorm();
779 template <
class Config_t>
780 static void normalize_impl (
const Eigen::MatrixBase<Config_t>& qout)
782 Config_t & qout_ = PINOCCHIO_EIGEN_CONST_CAST(Config_t,qout);
783 qout_.template tail<4>().normalize();
786 template <
class Config_t>
787 void random_impl (
const Eigen::MatrixBase<Config_t>& qout)
const
789 R3crossSO3_t().random(qout);
792 template <
class ConfigL_t,
class ConfigR_t,
class ConfigOut_t>
793 void randomConfiguration_impl(
const Eigen::MatrixBase<ConfigL_t> & lower,
794 const Eigen::MatrixBase<ConfigR_t> & upper,
795 const Eigen::MatrixBase<ConfigOut_t> & qout)
798 R3crossSO3_t ().randomConfiguration(lower, upper, qout);
801 template <
class ConfigL_t,
class ConfigR_t>
802 static bool isSameConfiguration_impl(
const Eigen::MatrixBase<ConfigL_t> & q0,
803 const Eigen::MatrixBase<ConfigR_t> & q1,
806 return R3crossSO3_t().isSameConfiguration(q0, q1, prec);
812 #endif // ifndef __pinocchio_multibody_liegroup_special_euclidean_operation_hpp__