5 #ifndef __pinocchio_multibody_liegroup_special_orthogonal_operation_hpp__ 6 #define __pinocchio_multibody_liegroup_special_orthogonal_operation_hpp__ 10 #include "pinocchio/spatial/explog.hpp" 11 #include "pinocchio/math/quaternion.hpp" 12 #include "pinocchio/multibody/liegroup/liegroup-base.hpp" 13 #include "pinocchio/utils/static-if.hpp" 17 template<
int Dim,
typename Scalar,
int Options = 0>
21 template<
int Dim,
typename Scalar,
int Options>
25 template<
typename _Scalar,
int _Options>
37 template<
typename _Scalar,
int _Options >
48 template<
typename _Scalar,
int _Options>
50 :
public LieGroupBase< SpecialOrthogonalOperationTpl<2,_Scalar,_Options> >
53 typedef Eigen::Matrix<Scalar,2,2>
Matrix2;
54 typedef typename Eigen::NumTraits<Scalar>::Real
RealScalar;
56 template<
typename Matrix2Like>
58 log(
const Eigen::MatrixBase<Matrix2Like> & R)
61 EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix2Like,2,2);
63 const Scalar tr = R.trace();
65 static const Scalar PI_value = PI<Scalar>();
75 asin((R(1,0) - R(0,1)) /
Scalar(2)),
92 assert (theta == theta);
98 template<
typename Matrix2Like>
100 Jlog(
const Eigen::MatrixBase<Matrix2Like> &)
103 EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix2Like,2,2);
130 return std::string(
"SO(2)");
133 template <
class ConfigL_t,
class ConfigR_t,
class Tangent_t>
135 const Eigen::MatrixBase<ConfigR_t> &
q1,
136 const Eigen::MatrixBase<Tangent_t> &
d)
139 R(0,0) = R(1,1) = q0.dot(q1);
140 R(1,0) =
q0(0) *
q1(1) -
q0(1) *
q1(0);
145 template <ArgumentPosition arg,
class ConfigL_t,
class ConfigR_t,
class JacobianOut_t>
147 const Eigen::MatrixBase<ConfigR_t> &
q1,
148 const Eigen::MatrixBase<JacobianOut_t> &
J)
const 151 R(0,0) = R(1,1) = q0.dot(q1);
152 R(1,0) =
q0(0) *
q1(1) -
q0(1) *
q1(0);
159 template <
class ConfigIn_t,
class Velocity_t,
class ConfigOut_t>
161 const Eigen::MatrixBase<Velocity_t> &
v,
162 const Eigen::MatrixBase<ConfigOut_t> & qout)
170 Scalar cosOmega,sinOmega;
SINCOS(omega, &sinOmega, &cosOmega);
173 out << cosOmega * ca - sinOmega * sa,
174 sinOmega * ca + cosOmega * sa;
177 const Scalar norm2 = out.squaredNorm();
178 out *= (3 - norm2) / 2;
182 template <
class Config_t,
class Jacobian_t>
184 const Eigen::MatrixBase<Jacobian_t> &
J)
186 assert(J.rows() ==
nq() && J.cols() ==
nv() &&
"J is not of the right dimension");
191 template <
class Config_t,
class Tangent_t,
class JacobianOut_t>
193 const Eigen::MatrixBase<Tangent_t> & ,
194 const Eigen::MatrixBase<JacobianOut_t> &
J,
210 assert(
false &&
"Wrong Op requesed value");
215 template <
class Config_t,
class Tangent_t,
class JacobianOut_t>
217 const Eigen::MatrixBase<Tangent_t> & ,
218 const Eigen::MatrixBase<JacobianOut_t> &
J,
234 assert(
false &&
"Wrong Op requesed value");
239 template <
class Config_t,
class Tangent_t,
class JacobianIn_t,
class JacobianOut_t>
241 const Eigen::MatrixBase<Tangent_t> & ,
242 const Eigen::MatrixBase<JacobianIn_t> & Jin,
243 const Eigen::MatrixBase<JacobianOut_t> & Jout)
const 248 template <
class Config_t,
class Tangent_t,
class JacobianIn_t,
class JacobianOut_t>
250 const Eigen::MatrixBase<Tangent_t> & ,
251 const Eigen::MatrixBase<JacobianIn_t> & Jin,
252 const Eigen::MatrixBase<JacobianOut_t> & Jout)
const 257 template <
class Config_t,
class Tangent_t,
class Jacobian_t>
259 const Eigen::MatrixBase<Tangent_t> & ,
260 const Eigen::MatrixBase<Jacobian_t> & )
const {}
262 template <
class Config_t,
class Tangent_t,
class Jacobian_t>
264 const Eigen::MatrixBase<Tangent_t> & ,
265 const Eigen::MatrixBase<Jacobian_t> & )
const {}
269 template <
class ConfigL_t,
class ConfigR_t,
class ConfigOut_t>
271 const Eigen::MatrixBase<ConfigR_t> &
q1,
273 const Eigen::MatrixBase<ConfigOut_t>& qout)
277 assert ( std::abs(q0.norm() - 1) < 1e-8 &&
"initial configuration not normalized");
278 assert ( std::abs(q1.norm() - 1) < 1e-8 &&
"final configuration not normalized");
279 Scalar cosTheta = q0.dot(q1);
281 Scalar theta = atan2(sinTheta, cosTheta);
282 assert (fabs (sin (theta) - sinTheta) < 1e-8);
284 const Scalar PI_value = PI<Scalar>();
286 if (fabs (theta) > 1e-6 && fabs (theta) < PI_value - 1e-6)
288 out = (sin ((1-u)*theta)/sinTheta) * q0
289 + (sin ( u *theta)/sinTheta) * q1;
291 else if (fabs (theta) < 1e-6)
293 out = (1-
u) * q0 + u * q1;
298 SINCOS(theta0,&out[1],&out[0]);
302 template <
class Config_t>
309 template <
class Config_t>
313 const Scalar norm = qin.norm();
315 return abs(norm -
Scalar(1.0)) < prec;
318 template <
class Config_t>
323 const Scalar PI_value = PI<Scalar>();
325 SINCOS(angle, &out(1), &out(0));
328 template <
class ConfigL_t,
class ConfigR_t,
class ConfigOut_t>
330 const Eigen::MatrixBase<ConfigR_t> &,
331 const Eigen::MatrixBase<ConfigOut_t> & qout)
const 337 template<
typename _Scalar,
int _Options>
339 :
public LieGroupBase< SpecialOrthogonalOperationTpl<3,_Scalar,_Options> >
372 return std::string(
"SO(3)");
375 template <
class ConfigL_t,
class ConfigR_t,
class Tangent_t>
377 const Eigen::MatrixBase<ConfigR_t> &
q1,
378 const Eigen::MatrixBase<Tangent_t> &
d)
380 ConstQuaternionMap_t quat0 (q0.derived().data());
382 ConstQuaternionMap_t quat1 (q1.derived().data());
389 template <ArgumentPosition arg,
class ConfigL_t,
class ConfigR_t,
class JacobianOut_t>
391 const Eigen::MatrixBase<ConfigR_t> &
q1,
392 const Eigen::MatrixBase<JacobianOut_t> &
J)
const 396 ConstQuaternionMap_t quat0 (q0.derived().data());
398 ConstQuaternionMap_t quat1 (q1.derived().data());
401 const Quaternion_t quat_diff = quat0.conjugate() * quat1;
406 const Matrix3
R = (quat_diff).matrix();
409 }
else if (arg ==
ARG1) {
414 template <
class ConfigIn_t,
class Velocity_t,
class ConfigOut_t>
416 const Eigen::MatrixBase<Velocity_t> &
v,
417 const Eigen::MatrixBase<ConfigOut_t> & qout)
419 ConstQuaternionMap_t
quat(q.derived().data());
424 quat_map =
quat * pOmega;
429 template <
class Config_t,
class Jacobian_t>
431 const Eigen::MatrixBase<Jacobian_t> &
J)
433 assert(J.rows() ==
nq() && J.cols() ==
nv() &&
"J is not of the right dimension");
439 ConstQuaternionMap_t quat_map(q.derived().data());
442 Eigen::Matrix<Scalar,NQ,NV,JacobianPlainType::Options|Eigen::RowMajor> Jexp3QuatCoeffWise;
451 if(quat_map.coeffs()[3] >= 0.)
459 template <
class Config_t,
class Tangent_t,
class JacobianOut_t>
461 const Eigen::MatrixBase<Tangent_t> &
v,
462 const Eigen::MatrixBase<JacobianOut_t> &
J,
478 assert(
false &&
"Wrong Op requesed value");
483 template <
class Config_t,
class Tangent_t,
class JacobianOut_t>
485 const Eigen::MatrixBase<Tangent_t> &
v,
486 const Eigen::MatrixBase<JacobianOut_t> &
J,
492 Jexp3<SETTO>(
v, J.derived());
495 Jexp3<ADDTO>(
v, J.derived());
498 Jexp3<RMTO>(
v, J.derived());
501 assert(
false &&
"Wrong Op requesed value");
506 template <
class Config_t,
class Tangent_t,
class JacobianIn_t,
class JacobianOut_t>
508 const Eigen::MatrixBase<Tangent_t> &
v,
509 const Eigen::MatrixBase<JacobianIn_t> & Jin,
510 const Eigen::MatrixBase<JacobianOut_t> & J_out)
const 514 const Matrix3 Jtmp3 =
exp3(-v);
515 Jout.noalias() = Jtmp3 * Jin;
518 template <
class Config_t,
class Tangent_t,
class JacobianIn_t,
class JacobianOut_t>
520 const Eigen::MatrixBase<Tangent_t> &
v,
521 const Eigen::MatrixBase<JacobianIn_t> & Jin,
522 const Eigen::MatrixBase<JacobianOut_t> & J_out)
const 527 Jexp3<SETTO>(
v, Jtmp3);
528 Jout.noalias() = Jtmp3 * Jin;
531 template <
class Config_t,
class Tangent_t,
class Jacobian_t>
533 const Eigen::MatrixBase<Tangent_t> &
v,
534 const Eigen::MatrixBase<Jacobian_t> & J_out)
const 538 const Matrix3 Jtmp3 =
exp3(-v);
542 template <
class Config_t,
class Tangent_t,
class Jacobian_t>
544 const Eigen::MatrixBase<Tangent_t> &
v,
545 const Eigen::MatrixBase<Jacobian_t> & J_out)
const 550 Jexp3<SETTO>(
v, Jtmp3);
555 template <
class ConfigL_t,
class ConfigR_t,
class ConfigOut_t>
557 const Eigen::MatrixBase<ConfigR_t> &
q1,
559 const Eigen::MatrixBase<ConfigOut_t> & qout)
561 ConstQuaternionMap_t quat0 (q0.derived().data());
563 ConstQuaternionMap_t quat1 (q1.derived().data());
568 quat_map = quat0.slerp(u, quat1);
572 template <
class ConfigL_t,
class ConfigR_t>
574 const Eigen::MatrixBase<ConfigR_t> &
q1)
577 difference_impl(q0, q1, t);
578 return t.squaredNorm();
581 template <
class Config_t>
588 template <
class Config_t>
592 const Scalar norm = qin.norm();
594 return abs(norm -
Scalar(1.0)) < prec;
597 template <
class Config_t>
606 template <
class ConfigL_t,
class ConfigR_t,
class ConfigOut_t>
608 const Eigen::MatrixBase<ConfigR_t> &,
609 const Eigen::MatrixBase<ConfigOut_t> & qout)
const 614 template <
class ConfigL_t,
class ConfigR_t>
616 const Eigen::MatrixBase<ConfigR_t> &
q1,
619 ConstQuaternionMap_t quat1(q0.derived().data());
621 ConstQuaternionMap_t quat2(q1.derived().data());
630 #endif // ifndef __pinocchio_multibody_liegroup_special_orthogonal_operation_hpp__ JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > const Eigen::MatrixBase< TangentVectorType > & v
static bool isSameConfiguration_impl(const Eigen::MatrixBase< ConfigL_t > &q0, const Eigen::MatrixBase< ConfigR_t > &q1, const Scalar &prec)
Eigen::Map< const Quaternion_t > ConstQuaternionMap_t
int nv(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointNvVisitor to get the dimension of the joint tangent space...
static void integrateCoeffWiseJacobian_impl(const Eigen::MatrixBase< Config_t > &q, const Eigen::MatrixBase< Jacobian_t > &J)
Eigen::Matrix< Scalar, NQ, 1, Options > ConfigVector_t
void dIntegrateTransport_dv_impl(const Eigen::MatrixBase< Config_t > &, const Eigen::MatrixBase< Tangent_t > &v, const Eigen::MatrixBase< JacobianIn_t > &Jin, const Eigen::MatrixBase< JacobianOut_t > &J_out) const
static bool isNormalized_impl(const Eigen::MatrixBase< Config_t > &qin, const Scalar &prec)
bool defineSameRotation(const Eigen::QuaternionBase< D1 > &q1, const Eigen::QuaternionBase< D2 > &q2, const typename D1::RealScalar &prec=Eigen::NumTraits< typename D1::Scalar >::dummy_precision())
Check if two quaternions define the same rotations.
static void difference_impl(const Eigen::MatrixBase< ConfigL_t > &q0, const Eigen::MatrixBase< ConfigR_t > &q1, const Eigen::MatrixBase< Tangent_t > &d)
Eigen::Matrix< Scalar, NV, NV, Options > JacobianMatrix_t
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.
void dIntegrateTransport_dq_impl(const Eigen::MatrixBase< Config_t > &, const Eigen::MatrixBase< Tangent_t > &, const Eigen::MatrixBase< Jacobian_t > &) const
void dIntegrateTransport_dq_impl(const Eigen::MatrixBase< Config_t > &, const Eigen::MatrixBase< Tangent_t > &v, const Eigen::MatrixBase< Jacobian_t > &J_out) const
void random_impl(const Eigen::MatrixBase< Config_t > &qout) const
static void normalize_impl(const Eigen::MatrixBase< Config_t > &qout)
void dDifference_impl(const Eigen::MatrixBase< ConfigL_t > &q0, const Eigen::MatrixBase< ConfigR_t > &q1, const Eigen::MatrixBase< JacobianOut_t > &J) const
Eigen::Matrix< Scalar, NV, 1, Options > TangentVector_t
static void interpolate_impl(const Eigen::MatrixBase< ConfigL_t > &q0, const Eigen::MatrixBase< ConfigR_t > &q1, const Scalar &u, const Eigen::MatrixBase< ConfigOut_t > &qout)
static Index nv()
Get dimension of Lie Group tangent space.
void firstOrderNormalize(const Eigen::QuaternionBase< D > &q)
bool isNormalized(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorType > &q, const Scalar &prec=Eigen::NumTraits< Scalar >::dummy_precision())
Check whether a configuration vector is normalized within the given precision provided by prec...
#define PINOCCHIO_LIE_GROUP_TPL_PUBLIC_INTERFACE(Derived)
void Jlog3(const Eigen::QuaternionBase< QuaternionLike > &quat, const Eigen::MatrixBase< Matrix3Like > &Jlog)
Computes the Jacobian of log3 operator for a unit quaternion.
static void difference_impl(const Eigen::MatrixBase< ConfigL_t > &q0, const Eigen::MatrixBase< ConfigR_t > &q1, const Eigen::MatrixBase< Tangent_t > &d)
void exp3(const Eigen::MatrixBase< Vector3Like > &v, Eigen::QuaternionBase< QuaternionLike > &quat_out)
Exp: so3 -> SO3 (quaternion)
traits< LieGroupDerived >::Scalar Scalar
void random_impl(const Eigen::MatrixBase< Config_t > &qout) const
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > & q
#define PINOCCHIO_EIGEN_CONST_CAST(TYPE, OBJ)
Macro for an automatic const_cast.
static ConfigVector_t neutral()
void Jexp3CoeffWise(const Eigen::MatrixBase< Vector3Like > &v, const Eigen::MatrixBase< Matrix43Like > &Jexp)
Derivative of where is a small perturbation of at identity.
void dDifference_impl(const Eigen::MatrixBase< ConfigL_t > &q0, const Eigen::MatrixBase< ConfigR_t > &q1, const Eigen::MatrixBase< JacobianOut_t > &J) const
Eigen::Matrix< typename Vector3Like::Scalar, 3, 3, PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like)::Options > exp3(const Eigen::MatrixBase< Vector3Like > &v)
Exp: so3 -> SO3.
void dIntegrateTransport_dv_impl(const Eigen::MatrixBase< Config_t > &, const Eigen::MatrixBase< Tangent_t > &, const Eigen::MatrixBase< JacobianIn_t > &Jin, const Eigen::MatrixBase< JacobianOut_t > &Jout) const
static std::string name()
static Index nv()
Get dimension of Lie Group tangent space.
static void integrate_impl(const Eigen::MatrixBase< ConfigIn_t > &q, const Eigen::MatrixBase< Velocity_t > &v, const Eigen::MatrixBase< ConfigOut_t > &qout)
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorIn1 > const Eigen::MatrixBase< ConfigVectorIn2 > const Scalar & u
Eigen::NumTraits< Scalar >::Real RealScalar
static void normalize_impl(const Eigen::MatrixBase< Config_t > &qout)
Eigen::Matrix< typename QuaternionLike::Scalar, 3, 1, PINOCCHIO_EIGEN_PLAIN_TYPE(typename QuaternionLike::Vector3)::Options > log3(const Eigen::QuaternionBase< QuaternionLike > &quat, typename QuaternionLike::Scalar &theta)
Same as log3 but with a unit quaternion as input.
Eigen::Map< Quaternion_t > QuaternionMap_t
static Matrix2Like::Scalar log(const Eigen::MatrixBase< Matrix2Like > &R)
static Matrix2Like::Scalar Jlog(const Eigen::MatrixBase< Matrix2Like > &)
static void dIntegrate_dq_impl(const Eigen::MatrixBase< Config_t > &, const Eigen::MatrixBase< Tangent_t > &, const Eigen::MatrixBase< JacobianOut_t > &J, const AssignmentOperatorType op=SETTO)
static Scalar squaredDistance_impl(const Eigen::MatrixBase< ConfigL_t > &q0, const Eigen::MatrixBase< ConfigR_t > &q1)
static void interpolate_impl(const Eigen::MatrixBase< ConfigL_t > &q0, const Eigen::MatrixBase< ConfigR_t > &q1, const Scalar &u, const Eigen::MatrixBase< ConfigOut_t > &qout)
void dIntegrateTransport_dv_impl(const Eigen::MatrixBase< Config_t > &, const Eigen::MatrixBase< Tangent_t > &, const Eigen::MatrixBase< Jacobian_t > &) const
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorIn1 > const Eigen::MatrixBase< ConfigVectorIn2 > & q1
static std::string name()
void dIntegrateTransport_dv_impl(const Eigen::MatrixBase< Config_t > &, const Eigen::MatrixBase< Tangent_t > &v, const Eigen::MatrixBase< Jacobian_t > &J_out) const
void SINCOS(const S1 &a, S2 *sa, S3 *ca)
Computes sin/cos values of a given input scalar.
Eigen::Quaternion< Scalar > Quaternion_t
traits< SE3Tpl >::Vector3 Vector3
static void dIntegrate_dv_impl(const Eigen::MatrixBase< Config_t > &, const Eigen::MatrixBase< Tangent_t > &v, const Eigen::MatrixBase< JacobianOut_t > &J, const AssignmentOperatorType op=SETTO)
void uniformRandom(const Eigen::QuaternionBase< Derived > &q)
Uniformly random quaternion sphere.
static ConfigVector_t neutral()
Eigen::NumTraits< Scalar >::Real RealScalar
Eigen::Matrix< Scalar, 2, 2 > Matrix2
Main pinocchio namespace.
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorIn1 > & q0
PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorType) integrate(const ModelTpl< Scalar
Integrate a configuration vector for the specified model for a tangent vector during one unit time...
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.
#define PINOCCHIO_DEFAULT_QUATERNION_NORM_TOLERANCE_VALUE
Common traits structure to fully define base classes for CRTP.
if_then_else_impl< LhsType, RhsType, ThenType, ElseType >::ReturnType if_then_else(const ComparisonOperators op, const LhsType &lhs_value, const RhsType &rhs_value, const ThenType &then_value, const ElseType &else_value)
SE3Tpl< Scalar, Options > SE3
static bool isNormalized_impl(const Eigen::MatrixBase< Config_t > &qin, const Scalar &prec)
void dIntegrateTransport_dq_impl(const Eigen::MatrixBase< Config_t > &, const Eigen::MatrixBase< Tangent_t > &v, const Eigen::MatrixBase< JacobianIn_t > &Jin, const Eigen::MatrixBase< JacobianOut_t > &J_out) const
void dIntegrateTransport_dq_impl(const Eigen::MatrixBase< Config_t > &, const Eigen::MatrixBase< Tangent_t > &, const Eigen::MatrixBase< JacobianIn_t > &Jin, const Eigen::MatrixBase< JacobianOut_t > &Jout) const
static void integrateCoeffWiseJacobian_impl(const Eigen::MatrixBase< Config_t > &q, const Eigen::MatrixBase< Jacobian_t > &J)
void randomConfiguration_impl(const Eigen::MatrixBase< ConfigL_t > &, const Eigen::MatrixBase< ConfigR_t > &, const Eigen::MatrixBase< ConfigOut_t > &qout) const
traits< SE3Tpl >::Matrix3 Matrix3
static void integrate_impl(const Eigen::MatrixBase< ConfigIn_t > &q, const Eigen::MatrixBase< Velocity_t > &v, const Eigen::MatrixBase< ConfigOut_t > &qout)
static void dIntegrate_dv_impl(const Eigen::MatrixBase< Config_t > &, const Eigen::MatrixBase< Tangent_t > &, const Eigen::MatrixBase< JacobianOut_t > &J, const AssignmentOperatorType op=SETTO)
void randomConfiguration_impl(const Eigen::MatrixBase< ConfigL_t > &, const Eigen::MatrixBase< ConfigR_t > &, const Eigen::MatrixBase< ConfigOut_t > &qout) const
static void dIntegrate_dq_impl(const Eigen::MatrixBase< Config_t > &, const Eigen::MatrixBase< Tangent_t > &v, const Eigen::MatrixBase< JacobianOut_t > &J, const AssignmentOperatorType op=SETTO)