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 PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix3Like, M, 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);
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);
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);
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 static bool isNormalized_impl(
const Eigen::MatrixBase<Config_t> & qin,
437 const Scalar norm = Scalar(qin.template tail<2>().norm());
439 return abs(norm - Scalar(1.0)) < prec;
442 template <
class Config_t>
443 void random_impl(
const Eigen::MatrixBase<Config_t> & qout)
const 445 R2crossSO2_t().random(qout);
448 template <
class ConfigL_t,
class ConfigR_t,
class ConfigOut_t>
449 void randomConfiguration_impl(
const Eigen::MatrixBase<ConfigL_t> & lower,
450 const Eigen::MatrixBase<ConfigR_t> & upper,
451 const Eigen::MatrixBase<ConfigOut_t> & qout)
454 R2crossSO2_t ().randomConfiguration(lower, upper, qout);
457 template <
class ConfigL_t,
class ConfigR_t>
458 static bool isSameConfiguration_impl(
const Eigen::MatrixBase<ConfigL_t> & q0,
459 const Eigen::MatrixBase<ConfigR_t> & q1,
462 return R2crossSO2_t().isSameConfiguration(q0, q1, prec);
467 template<
typename Matrix2Like,
typename Vector2Like,
typename Vector4Like>
469 const Eigen::MatrixBase<Vector2Like> & t,
470 const Eigen::MatrixBase<Vector4Like> & q)
472 EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(Matrix2Like, Matrix2);
473 EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(Vector2Like, Vector2);
474 EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigVector_t,Vector4Like);
476 PINOCCHIO_EIGEN_CONST_CAST(Vector2Like,t) = q.template head<2>();
477 const typename Vector4Like::Scalar & c_theta = q(2),
479 PINOCCHIO_EIGEN_CONST_CAST(Matrix2Like,R) << c_theta, -s_theta, s_theta, c_theta;
484 template<
typename _Scalar,
int _Options>
487 typedef _Scalar Scalar;
497 template<
typename _Scalar,
int _Options>
499 :
public LieGroupBase <SpecialEuclideanOperationTpl<3,_Scalar,_Options> >
505 typedef Eigen::Quaternion<Scalar,Options> Quaternion_t;
506 typedef Eigen::Map< Quaternion_t> QuaternionMap_t;
507 typedef Eigen::Map<const Quaternion_t> ConstQuaternionMap_t;
510 typedef typename Eigen::NumTraits<Scalar>::Real RealScalar;
526 static ConfigVector_t
neutral()
528 ConfigVector_t n; n.template head<6>().setZero(); n[6] = 1;
532 static std::string
name()
534 return std::string(
"SE(3)");
537 template <
class ConfigL_t,
class ConfigR_t,
class Tangent_t>
538 static void difference_impl(
const Eigen::MatrixBase<ConfigL_t> & q0,
539 const Eigen::MatrixBase<ConfigR_t> & q1,
540 const Eigen::MatrixBase<Tangent_t> & d)
542 ConstQuaternionMap_t quat0 (q0.derived().template tail<4>().data());
544 ConstQuaternionMap_t quat1 (q1.derived().template tail<4>().data());
547 PINOCCHIO_EIGEN_CONST_CAST(Tangent_t,d)
548 =
log6( SE3(quat0.matrix(), q0.derived().template head<3>()).inverse()
549 * SE3(quat1.matrix(), q1.derived().template head<3>())).toVector();
553 template <ArgumentPosition arg,
class ConfigL_t,
class ConfigR_t,
class JacobianOut_t>
555 const Eigen::MatrixBase<ConfigR_t> & q1,
556 const Eigen::MatrixBase<JacobianOut_t> & J)
const 558 typedef typename SE3::Vector3 Vector3;
559 typedef typename SE3::Matrix3 Matrix3;
561 ConstQuaternionMap_t quat0 (q0.derived().template tail<4>().data());
563 ConstQuaternionMap_t quat1 (q1.derived().template tail<4>().data());
566 Matrix3 R0(quat0.matrix()), R1 (quat1.matrix());
569 const SE3 M ( SE3(R0, q0.template head<3>()).inverse()
570 * SE3(R1, q1.template head<3>()));
576 const Vector3 p1_p0 = R1.transpose()*(q1.template head<3>() - q0.template head<3>());
578 JacobianOut_t & J0 = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J);
579 J0.template bottomRightCorner<3,3> ().noalias() = J0.template topLeftCorner <3,3> ().noalias() = - M.rotation().transpose();
580 J0.template topRightCorner<3,3> ().noalias() =
skew(p1_p0) * M.rotation().transpose();
581 J0.template bottomLeftCorner<3,3> ().setZero();
582 J0.applyOnTheLeft(J1);
584 else if (arg == ARG1) {
589 template <
class ConfigIn_t,
class Velocity_t,
class ConfigOut_t>
590 static void integrate_impl(
const Eigen::MatrixBase<ConfigIn_t> & q,
591 const Eigen::MatrixBase<Velocity_t> & v,
592 const Eigen::MatrixBase<ConfigOut_t> & qout)
594 ConfigOut_t & out = PINOCCHIO_EIGEN_CONST_CAST(ConfigOut_t,qout);
595 ConstQuaternionMap_t quat(q.derived().template tail<4>().data());
597 QuaternionMap_t res_quat (out.template tail<4>().data());
599 using internal::if_then_else;
601 SE3 M0 (quat.matrix(), q.derived().template head<3>());
603 SE3 M1 (M0 *
exp6(mref_v));
605 out.template head<3>() = M1.translation();
606 quaternion::assignQuaternion(res_quat,M1.rotation());
607 const Scalar dot_product = res_quat.dot(quat);
608 for(Eigen::DenseIndex k = 0; k < 4; ++k)
610 res_quat.coeffs().coeffRef(k) = if_then_else(internal::LT, dot_product, Scalar(0),
611 -res_quat.coeffs().coeff(k),
612 res_quat.coeffs().coeff(k));
621 template <
class Config_t,
class Jacobian_t>
622 static void integrateCoeffWiseJacobian_impl(
const Eigen::MatrixBase<Config_t> & q,
623 const Eigen::MatrixBase<Jacobian_t> & J)
625 assert(J.rows() ==
nq() && J.cols() ==
nv() &&
"J is not of the right dimension");
627 typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(Config_t) ConfigPlainType;
628 typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(Jacobian_t) JacobianPlainType;
629 typedef typename ConfigPlainType::Scalar Scalar;
631 Jacobian_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t,J);
634 ConstQuaternionMap_t quat_map(q.derived().template tail<4>().data());
636 Jout.template topLeftCorner<3,3>() = quat_map.toRotationMatrix();
639 typedef Eigen::Matrix<Scalar,4,3,JacobianPlainType::Options|Eigen::RowMajor> Jacobian43;
641 Jacobian43 Jexp3QuatCoeffWise;
646 typename SE3::Matrix3 Jlog;
653 if(quat_map.coeffs()[3] >= Scalar(0))
654 PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t,J).template bottomRightCorner<4,3>().noalias() = Jexp3QuatCoeffWise * Jlog;
656 PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t,J).template bottomRightCorner<4,3>().noalias() = -Jexp3QuatCoeffWise * Jlog;
659 template <
class Config_t,
class Tangent_t,
class JacobianOut_t>
660 static void dIntegrate_dq_impl(
const Eigen::MatrixBase<Config_t > & ,
661 const Eigen::MatrixBase<Tangent_t> & v,
662 const Eigen::MatrixBase<JacobianOut_t>& J,
663 const AssignmentOperatorType op=SETTO)
665 JacobianOut_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J);
679 assert(
false &&
"Wrong Op requesed value");
684 template <
class Config_t,
class Tangent_t,
class JacobianOut_t>
685 static void dIntegrate_dv_impl(
const Eigen::MatrixBase<Config_t > & ,
686 const Eigen::MatrixBase<Tangent_t> & v,
687 const Eigen::MatrixBase<JacobianOut_t>& J,
688 const AssignmentOperatorType op=SETTO)
702 assert(
false &&
"Wrong Op requesed value");
707 template <
class Config_t,
class Tangent_t,
class JacobianIn_t,
class JacobianOut_t>
708 void dIntegrateTransport_dq_impl(
const Eigen::MatrixBase<Config_t > & ,
709 const Eigen::MatrixBase<Tangent_t> & v,
710 const Eigen::MatrixBase<JacobianIn_t> & Jin,
711 const Eigen::MatrixBase<JacobianOut_t> & J_out)
const 713 JacobianOut_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J_out);
714 Eigen::Matrix<Scalar,6,6> Jtmp6;
717 Jout.template topRows<3>().noalias()
718 = Jtmp6.template topLeftCorner<3,3>() * Jin.template topRows<3>();
719 Jout.template topRows<3>().noalias()
720 += Jtmp6.template topRightCorner<3,3>() * Jin.template bottomRows<3>();
721 Jout.template bottomRows<3>().noalias()
722 = Jtmp6.template bottomRightCorner<3,3>() * Jin.template bottomRows<3>();
725 template <
class Config_t,
class Tangent_t,
class JacobianIn_t,
class JacobianOut_t>
726 void dIntegrateTransport_dv_impl(
const Eigen::MatrixBase<Config_t > & ,
727 const Eigen::MatrixBase<Tangent_t> & v,
728 const Eigen::MatrixBase<JacobianIn_t> & Jin,
729 const Eigen::MatrixBase<JacobianOut_t> & J_out)
const 731 JacobianOut_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J_out);
732 Eigen::Matrix<Scalar,6,6> Jtmp6;
735 Jout.template topRows<3>().noalias()
736 = Jtmp6.template topLeftCorner<3,3>() * Jin.template topRows<3>();
737 Jout.template topRows<3>().noalias()
738 += Jtmp6.template topRightCorner<3,3>() * Jin.template bottomRows<3>();
739 Jout.template bottomRows<3>().noalias()
740 = Jtmp6.template bottomRightCorner<3,3>() * Jin.template bottomRows<3>();
744 template <
class Config_t,
class Tangent_t,
class Jacobian_t>
745 void dIntegrateTransport_dq_impl(
const Eigen::MatrixBase<Config_t > & ,
746 const Eigen::MatrixBase<Tangent_t> & v,
747 const Eigen::MatrixBase<Jacobian_t> & J_out)
const 749 Jacobian_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t,J_out);
750 Eigen::Matrix<Scalar,6,6> Jtmp6;
754 Jout.template topRows<3>()
755 = Jtmp6.template topLeftCorner<3,3>() * Jout.template topRows<3>();
756 Jout.template topRows<3>().noalias()
757 += Jtmp6.template topRightCorner<3,3>() * Jout.template bottomRows<3>();
758 Jout.template bottomRows<3>()
759 = Jtmp6.template bottomRightCorner<3,3>() * Jout.template bottomRows<3>();
762 template <
class Config_t,
class Tangent_t,
class Jacobian_t>
763 void dIntegrateTransport_dv_impl(
const Eigen::MatrixBase<Config_t > & ,
764 const Eigen::MatrixBase<Tangent_t> & v,
765 const Eigen::MatrixBase<Jacobian_t> & J_out)
const 767 Jacobian_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t,J_out);
768 Eigen::Matrix<Scalar,6,6> Jtmp6;
771 Jout.template topRows<3>()
772 = Jtmp6.template topLeftCorner<3,3>() * Jout.template topRows<3>();
773 Jout.template topRows<3>().noalias()
774 += Jtmp6.template topRightCorner<3,3>() * Jout.template bottomRows<3>();
775 Jout.template bottomRows<3>()
776 = Jtmp6.template bottomRightCorner<3,3>() * Jout.template bottomRows<3>();
779 template <
class ConfigL_t,
class ConfigR_t>
780 static Scalar squaredDistance_impl(
const Eigen::MatrixBase<ConfigL_t> & q0,
781 const Eigen::MatrixBase<ConfigR_t> & q1)
784 difference_impl(q0, q1, t);
785 return t.squaredNorm();
788 template <
class Config_t>
789 static void normalize_impl (
const Eigen::MatrixBase<Config_t>& qout)
791 Config_t & qout_ = PINOCCHIO_EIGEN_CONST_CAST(Config_t,qout);
795 template <
class Config_t>
796 static bool isNormalized_impl(
const Eigen::MatrixBase<Config_t>& qin,
799 Scalar norm = Scalar(qin.template tail<4>().norm());
801 return abs(norm - Scalar(1.0)) < prec;
804 template <
class Config_t>
805 void random_impl (
const Eigen::MatrixBase<Config_t>& qout)
const 807 R3crossSO3_t().random(qout);
810 template <
class ConfigL_t,
class ConfigR_t,
class ConfigOut_t>
811 void randomConfiguration_impl(
const Eigen::MatrixBase<ConfigL_t> & lower,
812 const Eigen::MatrixBase<ConfigR_t> & upper,
813 const Eigen::MatrixBase<ConfigOut_t> & qout)
816 R3crossSO3_t ().randomConfiguration(lower, upper, qout);
819 template <
class ConfigL_t,
class ConfigR_t>
820 static bool isSameConfiguration_impl(
const Eigen::MatrixBase<ConfigL_t> & q0,
821 const Eigen::MatrixBase<ConfigR_t> & q1,
824 return R3crossSO3_t().isSameConfiguration(q0, q1, prec);
830 #endif // ifndef __pinocchio_multibody_liegroup_special_euclidean_operation_hpp__
int nv(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointNvVisitor to get the dimension of the joint tangent space...
static Index nv()
Get dimension of Lie Group tangent space.
static Index nv()
Get dimension of Lie Group 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 Jlog3(const Scalar &theta, const Eigen::MatrixBase< Vector3Like > &log, const Eigen::MatrixBase< Matrix3Like > &Jlog)
Derivative of log3.
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.
std::string name(const LieGroupGenericTpl< LieGroupCollection > &lg)
Visit a LieGroupVariant to get the name of it.
void firstOrderNormalize(const Eigen::QuaternionBase< D > &q)
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 Jexp6(const MotionDense< MotionDerived > &nu, const Eigen::MatrixBase< Matrix6Like > &Jexp)
Derivative of exp6 Computed as the inverse of Jlog6.
bool isUnitary(const Eigen::MatrixBase< MatrixLike > &mat, const typename MatrixLike::RealScalar &prec=Eigen::NumTraits< typename MatrixLike::Scalar >::dummy_precision())
Check whether the input matrix is Unitary within the given precision.
void Jexp3CoeffWise(const Eigen::MatrixBase< Vector3Like > &v, const Eigen::MatrixBase< Matrix43Like > &Jexp)
Derivative of where is a small perturbation of at identity.
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.
void SINCOS(const S1 &a, S2 *sa, S3 *ca)
Computes sin/cos values of a given input scalar.
Main pinocchio namespace.
void dDifference_impl(const Eigen::MatrixBase< ConfigL_t > &q0, const Eigen::MatrixBase< ConfigR_t > &q1, const Eigen::MatrixBase< JacobianOut_t > &J) const
void neutral(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ReturnType > &qout)
Return the neutral configuration element related to the model configuration space.
bool isNormalized(const Eigen::QuaternionBase< Quaternion > &quat, const typename Quaternion::Coefficients::RealScalar &prec=Eigen::NumTraits< typename Quaternion::Coefficients::RealScalar >::dummy_precision())
Check whether the input quaternion is Normalized within the given precision.
Common traits structure to fully define base classes for CRTP.
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...
void normalize(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorType > &qout)
Normalize a configuration vector.
SE3Tpl< typename MotionDerived::Scalar, typename MotionDerived::Vector3 ::Options > exp6(const MotionDense< MotionDerived > &nu)
Exp: se3 -> SE3.