5 #ifndef __pinocchio_special_euclidean_operation_hpp__ 6 #define __pinocchio_special_euclidean_operation_hpp__ 10 #include "pinocchio/macros.hpp" 11 #include "pinocchio/spatial/fwd.hpp" 12 #include "pinocchio/utils/static-if.hpp" 13 #include "pinocchio/spatial/se3.hpp" 14 #include "pinocchio/multibody/liegroup/liegroup-base.hpp" 16 #include "pinocchio/multibody/liegroup/vector-space.hpp" 17 #include "pinocchio/multibody/liegroup/cartesian-product.hpp" 18 #include "pinocchio/multibody/liegroup/special-orthogonal.hpp" 22 template<
int Dim,
typename Scalar,
int Options = 0>
26 template<
int Dim,
typename Scalar,
int Options>
30 template<
typename _Scalar,
int _Options>
33 typedef _Scalar Scalar;
43 template<
typename _Scalar,
int _Options>
45 :
public LieGroupBase <SpecialEuclideanOperationTpl<2,_Scalar,_Options> >
53 typedef Eigen::Matrix<Scalar,2,2,Options> Matrix2;
54 typedef Eigen::Matrix<Scalar,2,1,Options> Vector2;
56 template<
typename TangentVector,
typename Matrix2Like,
typename Vector2Like>
57 static void exp(
const Eigen::MatrixBase<TangentVector> & v,
58 const Eigen::MatrixBase<Matrix2Like> & R,
59 const Eigen::MatrixBase<Vector2Like> & t)
61 EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(TangentVector_t,TangentVector);
62 EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix2Like, 2, 2);
63 EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector2Like, 2);
65 typedef typename Matrix2Like::Scalar Scalar;
66 const Scalar omega = v(2);
67 Scalar cv,sv;
SINCOS(omega, &sv, &cv);
68 PINOCCHIO_EIGEN_CONST_CAST(Matrix2Like,R) << cv, -sv, sv, cv;
69 using internal::if_then_else;
72 typename PINOCCHIO_EIGEN_PLAIN_TYPE(Vector2Like) vcross(-v(1), v(0));
73 vcross -= -v(1)*R.col(0) + v(0)*R.col(1);
75 Scalar omega_abs = math::fabs(omega);
76 PINOCCHIO_EIGEN_CONST_CAST(Vector2Like,t).coeffRef(0) = if_then_else(omega_abs > 1e-14,
80 PINOCCHIO_EIGEN_CONST_CAST(Vector2Like,t).coeffRef(1) = if_then_else(omega_abs > 1e-14,
87 template<
typename Matrix2Like,
typename Vector2Like,
typename Matrix3Like>
88 static void toInverseActionMatrix(
const Eigen::MatrixBase<Matrix2Like> & R,
89 const Eigen::MatrixBase<Vector2Like> & t,
90 const Eigen::MatrixBase<Matrix3Like> & M)
92 EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix2Like, 2, 2);
93 EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector2Like, 2);
94 EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix3Like, 3, 3);
96 typedef typename Matrix3Like::Scalar Scalar;
98 Matrix3Like & Mout = PINOCCHIO_EIGEN_CONST_CAST(Matrix3Like,M);
99 Mout.template topLeftCorner<2,2>() = R.transpose();
100 typename PINOCCHIO_EIGEN_PLAIN_TYPE(Vector2Like) tinv(R.transpose() * t);
101 Mout.template topRightCorner<2,1>() << - tinv(1), tinv(0);
102 Mout.template bottomLeftCorner<1,2>().setZero();
103 Mout(2,2) = (Scalar)1;
106 template<
typename Matrix2Like,
typename Vector2Like,
typename TangentVector>
107 static void log(
const Eigen::MatrixBase<Matrix2Like> & R,
108 const Eigen::MatrixBase<Vector2Like> & p,
109 const Eigen::MatrixBase<TangentVector> & v)
111 EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix2Like, 2, 2);
112 EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector2Like, 2);
113 EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(TangentVector_t,TangentVector);
115 TangentVector & vout = PINOCCHIO_EIGEN_CONST_CAST(TangentVector,v);
117 typedef typename Matrix2Like::Scalar Scalar1;
119 Scalar1 t = SO2_t::log(R);
120 const Scalar1 tabs = math::fabs(t);
121 const Scalar1 t2 = t*t;
125 alpha = 1 - t2/12 - t2*t2/720;
129 Scalar1 st,ct;
SINCOS(tabs, &st, &ct);
130 alpha = tabs*st/(2*(1-ct));
133 vout.template head<2>().noalias() = alpha * p;
134 vout(0) += t/2 * p(1);
135 vout(1) += -t/2 * p(0);
139 template<
typename Matrix2Like,
typename Vector2Like,
typename JacobianOutLike>
140 static void Jlog(
const Eigen::MatrixBase<Matrix2Like> & R,
141 const Eigen::MatrixBase<Vector2Like> & p,
142 const Eigen::MatrixBase<JacobianOutLike> & J)
144 EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix2Like, 2, 2);
145 EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector2Like, 2);
146 EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(JacobianOutLike, JacobianMatrix_t);
148 JacobianOutLike & Jout = PINOCCHIO_EIGEN_CONST_CAST(JacobianOutLike,J);
150 typedef typename Matrix2Like::Scalar Scalar1;
152 Scalar1 t = SO2_t::log(R);
153 const Scalar1 tabs = math::fabs(t);
154 Scalar1 alpha, alpha_dot;
159 alpha_dot = - t / 6 - t2*t / 180;
163 Scalar1 st,ct;
SINCOS(t, &st, &ct);
164 Scalar1 inv_2_1_ct = 0.5 / (1-ct);
166 alpha = t*st*inv_2_1_ct;
168 alpha_dot = (st-t) * inv_2_1_ct;
171 typename PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix2Like) V;
172 V(0,0) = V(1,1) = alpha;
176 Jout.template topLeftCorner <2,2>().noalias() = V * R;
177 Jout.template topRightCorner<2,1>() << alpha_dot*p[0] + p[1]/2, -p(0)/2 + alpha_dot*p(1);
178 Jout.template bottomLeftCorner<1,2>().setZero();
196 static ConfigVector_t
neutral()
198 ConfigVector_t n; n << Scalar(0), Scalar(0), Scalar(1), Scalar(0);
202 static std::string
name()
204 return std::string(
"SE(2)");
207 template <
class ConfigL_t,
class ConfigR_t,
class Tangent_t>
208 static void difference_impl(
const Eigen::MatrixBase<ConfigL_t> & q0,
209 const Eigen::MatrixBase<ConfigR_t> & q1,
210 const Eigen::MatrixBase<Tangent_t> & d)
214 PINOCCHIO_EIGEN_CONST_CAST(Tangent_t,d).setZero();
217 Matrix2 R0, R1; Vector2 t0, t1;
220 Matrix2 R (R0.transpose() * R1);
221 Vector2 t (R0.transpose() * (t1 - t0));
226 template <ArgumentPosition arg,
class ConfigL_t,
class ConfigR_t,
class JacobianOut_t>
227 void dDifference_impl (
const Eigen::MatrixBase<ConfigL_t> & q0,
228 const Eigen::MatrixBase<ConfigR_t> & q1,
229 const Eigen::MatrixBase<JacobianOut_t>& J)
const 231 Matrix2 R0, R1; Vector2 t0, t1;
234 Matrix2 R (R0.transpose() * R1);
235 Vector2 t (R0.transpose() * (t1 - t0));
242 Vector2 pcross (q1(1) - q0(1), q0(0) - q1(0));
244 JacobianOut_t& J0 = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, J);
245 J0.template topLeftCorner <2,2> ().noalias() = - R.transpose();
246 J0.template topRightCorner<2,1> ().noalias() = R1.transpose() * pcross;
247 J0.template bottomLeftCorner <1,2> ().setZero();
249 J0.applyOnTheLeft(J1);
250 }
else if (arg == ARG1) {
255 template <
class ConfigIn_t,
class Velocity_t,
class ConfigOut_t>
256 static void integrate_impl(
const Eigen::MatrixBase<ConfigIn_t> & q,
257 const Eigen::MatrixBase<Velocity_t> & v,
258 const Eigen::MatrixBase<ConfigOut_t> & qout)
260 ConfigOut_t & out = PINOCCHIO_EIGEN_CONST_CAST(ConfigOut_t,qout);
267 out.template head<2>().noalias() = R0 * t + t0;
268 out.template tail<2>().noalias() = R0 * R.col(0);
271 template <
class Config_t,
class Jacobian_t>
272 static void integrateCoeffWiseJacobian_impl(
const Eigen::MatrixBase<Config_t> & q,
273 const Eigen::MatrixBase<Jacobian_t> & J)
275 assert(J.rows() ==
nq() && J.cols() ==
nv() &&
"J is not of the right dimension");
277 Jacobian_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t,J);
280 const typename Config_t::Scalar & c_theta = q(2),
283 Jout.template topLeftCorner<2,2>() << c_theta, -s_theta, s_theta, c_theta;
284 Jout.template bottomRightCorner<2,1>() << -s_theta, c_theta;
287 template <
class Config_t,
class Tangent_t,
class JacobianOut_t>
288 static void dIntegrate_dq_impl(
const Eigen::MatrixBase<Config_t > & ,
289 const Eigen::MatrixBase<Tangent_t> & v,
290 const Eigen::MatrixBase<JacobianOut_t>& J)
292 JacobianOut_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J);
298 toInverseActionMatrix(R, t, Jout);
301 template <
class Config_t,
class Tangent_t,
class JacobianOut_t>
302 static void dIntegrate_dv_impl(
const Eigen::MatrixBase<Config_t > & ,
303 const Eigen::MatrixBase<Tangent_t> & v,
304 const Eigen::MatrixBase<JacobianOut_t> & J)
306 JacobianOut_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J);
309 Eigen::Matrix<Scalar,6,6> Jtmp6;
311 Jout << Jtmp6.template topLeftCorner<2,2>(), Jtmp6.template topRightCorner<2,1>(),
312 Jtmp6.template bottomLeftCorner<1,2>(), Jtmp6.template bottomRightCorner<1,1>();
325 template <
class Config_t>
326 static void normalize_impl (
const Eigen::MatrixBase<Config_t>& qout)
328 PINOCCHIO_EIGEN_CONST_CAST(Config_t,qout).template tail<2>().
normalize();
331 template <
class Config_t>
332 void random_impl (
const Eigen::MatrixBase<Config_t>& qout)
const 334 R2crossSO2_t().random(qout);
337 template <
class ConfigL_t,
class ConfigR_t,
class ConfigOut_t>
338 void randomConfiguration_impl(
const Eigen::MatrixBase<ConfigL_t> & lower,
339 const Eigen::MatrixBase<ConfigR_t> & upper,
340 const Eigen::MatrixBase<ConfigOut_t> & qout)
343 R2crossSO2_t ().randomConfiguration(lower, upper, qout);
346 template <
class ConfigL_t,
class ConfigR_t>
347 static bool isSameConfiguration_impl(
const Eigen::MatrixBase<ConfigL_t> & q0,
348 const Eigen::MatrixBase<ConfigR_t> & q1,
351 return R2crossSO2_t().isSameConfiguration(q0, q1, prec);
356 template<
typename Matrix2Like,
typename Vector2Like,
typename Vector4Like>
358 const Eigen::MatrixBase<Vector2Like> & t,
359 const Eigen::MatrixBase<Vector4Like> & q)
361 EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(Matrix2Like, Matrix2);
362 EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(Vector2Like, Vector2);
363 EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigVector_t,Vector4Like);
365 PINOCCHIO_EIGEN_CONST_CAST(Vector2Like,t) = q.template head<2>();
366 const typename Vector4Like::Scalar & c_theta = q(2),
368 PINOCCHIO_EIGEN_CONST_CAST(Matrix2Like,R) << c_theta, -s_theta, s_theta, c_theta;
373 template<
typename _Scalar,
int _Options>
376 typedef _Scalar Scalar;
386 template<
typename _Scalar,
int _Options>
388 :
public LieGroupBase <SpecialEuclideanOperationTpl<3,_Scalar,_Options> >
394 typedef Eigen::Quaternion<Scalar,Options> Quaternion_t;
395 typedef Eigen::Map< Quaternion_t> QuaternionMap_t;
396 typedef Eigen::Map<const Quaternion_t> ConstQuaternionMap_t;
414 static ConfigVector_t
neutral()
416 ConfigVector_t n; n.template head<6>().setZero(); n[6] = 1;
420 static std::string
name()
422 return std::string(
"SE(3)");
425 template <
class ConfigL_t,
class ConfigR_t,
class Tangent_t>
426 static void difference_impl(
const Eigen::MatrixBase<ConfigL_t> & q0,
427 const Eigen::MatrixBase<ConfigR_t> & q1,
428 const Eigen::MatrixBase<Tangent_t> & d)
432 PINOCCHIO_EIGEN_CONST_CAST(Tangent_t,d).setZero();
435 ConstQuaternionMap_t p0 (q0.derived().template tail<4>().data());
436 ConstQuaternionMap_t p1 (q1.derived().template tail<4>().data());
437 PINOCCHIO_EIGEN_CONST_CAST(Tangent_t,d)
438 =
log6( SE3(p0.matrix(), q0.derived().template head<3>()).inverse()
439 * SE3(p1.matrix(), q1.derived().template head<3>())).toVector();
443 template <ArgumentPosition arg,
class ConfigL_t,
class ConfigR_t,
class JacobianOut_t>
445 const Eigen::MatrixBase<ConfigR_t> & q1,
446 const Eigen::MatrixBase<JacobianOut_t>& J)
const 448 ConstQuaternionMap_t p0 (q0.derived().template tail<4>().data());
449 ConstQuaternionMap_t p1 (q1.derived().template tail<4>().data());
450 typename SE3::Matrix3 R0 (p0.matrix()),
452 SE3 M ( SE3(R0, q0.derived().template head<3>()).inverse()
453 * SE3(R1, q1.derived().template head<3>()));
459 typename SE3::Vector3 p1_p0 ( q1.derived().template head<3>()
460 - q0.derived().template head<3>());
462 JacobianOut_t& J0 = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J);
463 J0.template topLeftCorner <3,3> ().noalias() = - M.rotation().transpose();
464 J0.template topRightCorner<3,3> ().noalias() = R1.transpose() *
skew (p1_p0) * R0;
465 J0.template bottomLeftCorner <3,3> ().setZero();
466 J0.template bottomRightCorner<3,3> ().noalias() = - M.rotation().transpose();
467 J0.applyOnTheLeft(J1);
468 }
else if (arg == ARG1) {
473 template <
class ConfigIn_t,
class Velocity_t,
class ConfigOut_t>
474 static void integrate_impl(
const Eigen::MatrixBase<ConfigIn_t> & q,
475 const Eigen::MatrixBase<Velocity_t> & v,
476 const Eigen::MatrixBase<ConfigOut_t> & qout)
478 ConfigOut_t & out = PINOCCHIO_EIGEN_CONST_CAST(ConfigOut_t,qout);
479 ConstQuaternionMap_t quat(q.derived().template tail<4>().data());
480 QuaternionMap_t res_quat (out.template tail<4>().data());
482 using internal::if_then_else;
484 SE3 M0 (quat.matrix(), q.derived().template head<3>());
486 SE3 M1 (M0 *
exp6(mref_v));
488 out.template head<3>() = M1.translation();
489 quaternion::assignQuaternion(res_quat,M1.rotation());
490 const Scalar dot_product = res_quat.dot(quat);
491 for(Eigen::DenseIndex k = 0; k < 4; ++k)
493 res_quat.coeffs().coeffRef(k) = if_then_else(dot_product < 0,
494 -res_quat.coeffs().coeff(k),
495 res_quat.coeffs().coeff(k));
503 template <
class Config_t,
class Jacobian_t>
504 static void integrateCoeffWiseJacobian_impl(
const Eigen::MatrixBase<Config_t> & q,
505 const Eigen::MatrixBase<Jacobian_t> & J)
507 assert(J.rows() ==
nq() && J.cols() ==
nv() &&
"J is not of the right dimension");
509 typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(Config_t) ConfigPlainType;
510 typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(Jacobian_t) JacobianPlainType;
511 typedef typename ConfigPlainType::Scalar Scalar;
512 Jacobian_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t,J);
515 ConstQuaternionMap_t quat_map(q.derived().template tail<4>().data());
516 Jout.template topLeftCorner<3,3>() = quat_map.toRotationMatrix();
519 typedef Eigen::Matrix<Scalar,4,3,JacobianPlainType::Options|Eigen::RowMajor> Jacobian43;
521 Jacobian43 Jexp3QuatCoeffWise;
526 typename SE3::Matrix3 Jlog;
533 if(quat_map.coeffs()[3] >= Scalar(0))
534 PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t,J).template bottomRightCorner<4,3>().noalias() = Jexp3QuatCoeffWise * Jlog;
536 PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t,J).template bottomRightCorner<4,3>().noalias() = -Jexp3QuatCoeffWise * Jlog;
539 template <
class Config_t,
class Tangent_t,
class JacobianOut_t>
540 static void dIntegrate_dq_impl(
const Eigen::MatrixBase<Config_t > & ,
541 const Eigen::MatrixBase<Tangent_t> & v,
542 const Eigen::MatrixBase<JacobianOut_t>& J)
544 JacobianOut_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J);
548 template <
class Config_t,
class Tangent_t,
class JacobianOut_t>
549 static void dIntegrate_dv_impl(
const Eigen::MatrixBase<Config_t > & ,
550 const Eigen::MatrixBase<Tangent_t> & v,
551 const Eigen::MatrixBase<JacobianOut_t>& J)
565 template <
class ConfigL_t,
class ConfigR_t>
566 static Scalar squaredDistance_impl(
const Eigen::MatrixBase<ConfigL_t> & q0,
567 const Eigen::MatrixBase<ConfigR_t> & q1)
570 difference_impl(q0, q1, t);
571 return t.squaredNorm();
574 template <
class Config_t>
575 static void normalize_impl (
const Eigen::MatrixBase<Config_t>& qout)
577 Config_t & qout_ = PINOCCHIO_EIGEN_CONST_CAST(Config_t,qout);
581 template <
class Config_t>
582 void random_impl (
const Eigen::MatrixBase<Config_t>& qout)
const 584 R3crossSO3_t().random(qout);
587 template <
class ConfigL_t,
class ConfigR_t,
class ConfigOut_t>
588 void randomConfiguration_impl(
const Eigen::MatrixBase<ConfigL_t> & lower,
589 const Eigen::MatrixBase<ConfigR_t> & upper,
590 const Eigen::MatrixBase<ConfigOut_t> & qout)
593 R3crossSO3_t ().randomConfiguration(lower, upper, qout);
596 template <
class ConfigL_t,
class ConfigR_t>
597 static bool isSameConfiguration_impl(
const Eigen::MatrixBase<ConfigL_t> & q0,
598 const Eigen::MatrixBase<ConfigR_t> & q1,
601 return R3crossSO3_t().isSameConfiguration(q0, q1, prec);
607 #endif // ifndef __pinocchio_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 Jexp3CoeffWise(const Eigen::MatrixBase< Vector3Like > &v, const Eigen::MatrixBase< Matrix43Like > &Jexp)
Derivative of where is a small perturbation of at identity.
void Jexp6(const MotionDense< MotionDerived > &nu, const Eigen::MatrixBase< Matrix6Like > &Jexp)
Derivative of exp6 Computed as the inverse of Jlog6.
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.
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.