5 #ifndef __pinocchio_special_orthogonal_operation_hpp__ 6 #define __pinocchio_special_orthogonal_operation_hpp__ 10 #include <pinocchio/spatial/explog.hpp> 11 #include <pinocchio/math/quaternion.hpp> 12 #include <pinocchio/multibody/liegroup/liegroup-base.hpp> 16 template<
int Dim,
typename Scalar,
int Options = 0>
20 template<
int Dim,
typename Scalar,
int Options>
24 template<
typename _Scalar,
int _Options>
27 typedef _Scalar Scalar;
36 template<
typename _Scalar,
int _Options >
38 typedef _Scalar Scalar;
47 template<
typename _Scalar,
int _Options>
49 :
public LieGroupBase< SpecialOrthogonalOperationTpl<2,_Scalar,_Options> >
52 typedef Eigen::Matrix<Scalar,2,2> Matrix2;
54 template<
typename Matrix2Like>
55 static typename Matrix2Like::Scalar
56 log(
const Eigen::MatrixBase<Matrix2Like> & R)
59 typedef typename Matrix2Like::Scalar Scalar;
60 EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix2Like,2,2);
63 const Scalar tr = R.trace();
64 const bool pos = (R (1, 0) > Scalar(0));
65 const Scalar PI_value = PI<Scalar>();
66 if (tr > Scalar(2)) theta = Scalar(0);
67 else if (tr < Scalar(-2)) theta = (pos ? PI_value : -PI_value);
70 else if (tr > Scalar(2) - 1e-2) theta = asin ((R(1,0) - R(0,1)) / Scalar(2));
71 else theta = (pos ? acos (tr/Scalar(2)) : -acos(tr/Scalar(2)));
72 assert (theta == theta);
73 assert ((cos (theta) * R(0,0) + sin (theta) * R(1,0) > 0) &&
74 (cos (theta) * R(1,0) - sin (theta) * R(0,0) < 1e-6));
78 template<
typename Matrix2Like>
79 static typename Matrix2Like::Scalar
80 Jlog(
const Eigen::MatrixBase<Matrix2Like> &)
82 typedef typename Matrix2Like::Scalar Scalar;
83 EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix2Like,2,2);
102 static ConfigVector_t
neutral()
104 ConfigVector_t n; n << Scalar(1), Scalar(0);
108 static std::string
name()
110 return std::string(
"SO(2)");
113 template <
class ConfigL_t,
class ConfigR_t,
class Tangent_t>
114 static void difference_impl(
const Eigen::MatrixBase<ConfigL_t> & q0,
115 const Eigen::MatrixBase<ConfigR_t> & q1,
116 const Eigen::MatrixBase<Tangent_t> & d)
119 PINOCCHIO_EIGEN_CONST_CAST(Tangent_t,d).setZero();
123 R(0,0) = R(1,1) = q0.dot(q1);
124 R(1,0) = q0(0) * q1(1) - q0(1) * q1(0);
126 PINOCCHIO_EIGEN_CONST_CAST(Tangent_t,d)[0] = log(R);
129 template <ArgumentPosition arg,
class ConfigL_t,
class ConfigR_t,
class JacobianOut_t>
130 void dDifference_impl(
const Eigen::MatrixBase<ConfigL_t> & q0,
131 const Eigen::MatrixBase<ConfigR_t> & q1,
132 const Eigen::MatrixBase<JacobianOut_t> & J)
const 135 R(0,0) = R(1,1) = q0.dot(q1);
136 R(1,0) = q0(0) * q1(1) - q0(1) * q1(0);
140 PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J).coeffRef(0,0) = ((arg==ARG0) ? -w : w);
143 template <
class ConfigIn_t,
class Velocity_t,
class ConfigOut_t>
144 static void integrate_impl(
const Eigen::MatrixBase<ConfigIn_t> & q,
145 const Eigen::MatrixBase<Velocity_t> & v,
146 const Eigen::MatrixBase<ConfigOut_t> & qout)
148 ConfigOut_t & out = PINOCCHIO_EIGEN_CONST_CAST(ConfigOut_t,qout);
150 const Scalar & ca = q(0);
151 const Scalar & sa = q(1);
152 const Scalar & omega = v(0);
154 Scalar cosOmega,sinOmega;
SINCOS(omega, &sinOmega, &cosOmega);
157 out << cosOmega * ca - sinOmega * sa,
158 sinOmega * ca + cosOmega * sa;
161 const Scalar norm2 = out.squaredNorm();
162 out *= (3 - norm2) / 2;
165 template <
class Config_t,
class Jacobian_t>
166 static void integrateCoeffWiseJacobian_impl(
const Eigen::MatrixBase<Config_t> & q,
167 const Eigen::MatrixBase<Jacobian_t> & J)
169 assert(J.rows() ==
nq() && J.cols() ==
nv() &&
"J is not of the right dimension");
170 Jacobian_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t,J);
174 template <
class Config_t,
class Tangent_t,
class JacobianOut_t>
175 static void dIntegrate_dq_impl(
const Eigen::MatrixBase<Config_t > & ,
176 const Eigen::MatrixBase<Tangent_t> & ,
177 const Eigen::MatrixBase<JacobianOut_t>& J)
179 JacobianOut_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J);
183 template <
class Config_t,
class Tangent_t,
class JacobianOut_t>
184 static void dIntegrate_dv_impl(
const Eigen::MatrixBase<Config_t > & ,
185 const Eigen::MatrixBase<Tangent_t> & ,
186 const Eigen::MatrixBase<JacobianOut_t>& J)
188 JacobianOut_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J);
192 template <
class ConfigL_t,
class ConfigR_t,
class ConfigOut_t>
193 static void interpolate_impl(
const Eigen::MatrixBase<ConfigL_t> & q0,
194 const Eigen::MatrixBase<ConfigR_t> & q1,
196 const Eigen::MatrixBase<ConfigOut_t>& qout)
198 ConfigOut_t & out = PINOCCHIO_EIGEN_CONST_CAST(ConfigOut_t,qout);
200 assert ( std::abs(q0.norm() - 1) < 1e-8 &&
"initial configuration not normalized");
201 assert ( std::abs(q1.norm() - 1) < 1e-8 &&
"final configuration not normalized");
202 Scalar cosTheta = q0.dot(q1);
203 Scalar sinTheta = q0(0)*q1(1) - q0(1)*q1(0);
204 Scalar theta = atan2(sinTheta, cosTheta);
205 assert (fabs (sin (theta) - sinTheta) < 1e-8);
207 const Scalar PI_value = PI<Scalar>();
209 if (fabs (theta) > 1e-6 && fabs (theta) < PI_value - 1e-6)
211 out = (sin ((1-u)*theta)/sinTheta) * q0
212 + (sin ( u *theta)/sinTheta) * q1;
214 else if (fabs (theta) < 1e-6)
216 out = (1-u) * q0 + u * q1;
220 Scalar theta0 = atan2 (q0(1), q0(0));
221 SINCOS(theta0,&out[1],&out[0]);
229 template <
class Config_t>
230 static void normalize_impl (
const Eigen::MatrixBase<Config_t> & qout)
232 Config_t & qout_ = PINOCCHIO_EIGEN_CONST_CAST(Config_t,qout).derived();
236 template <
class Config_t>
237 void random_impl (
const Eigen::MatrixBase<Config_t>& qout)
const 239 Config_t & out = PINOCCHIO_EIGEN_CONST_CAST(Config_t,qout);
241 const Scalar PI_value = PI<Scalar>();
242 const Scalar angle = -PI_value + Scalar(2)* PI_value * ((Scalar)rand())/RAND_MAX;
243 SINCOS(angle, &out(1), &out(0));
246 template <
class ConfigL_t,
class ConfigR_t,
class ConfigOut_t>
247 void randomConfiguration_impl(
const Eigen::MatrixBase<ConfigL_t> &,
248 const Eigen::MatrixBase<ConfigR_t> &,
249 const Eigen::MatrixBase<ConfigOut_t> & qout)
256 template<
typename _Scalar,
int _Options>
258 :
public LieGroupBase< SpecialOrthogonalOperationTpl<3,_Scalar,_Options> >
262 typedef Eigen::Quaternion<Scalar> Quaternion_t;
263 typedef Eigen::Map< Quaternion_t> QuaternionMap_t;
264 typedef Eigen::Map<const Quaternion_t> ConstQuaternionMap_t;
281 static ConfigVector_t
neutral()
283 ConfigVector_t n; n.setZero (); n[3] = Scalar(1);
287 static std::string
name()
289 return std::string(
"SO(3)");
292 template <
class ConfigL_t,
class ConfigR_t,
class Tangent_t>
293 static void difference_impl(
const Eigen::MatrixBase<ConfigL_t> & q0,
294 const Eigen::MatrixBase<ConfigR_t> & q1,
295 const Eigen::MatrixBase<Tangent_t> & d)
298 PINOCCHIO_EIGEN_CONST_CAST(Tangent_t,d).setZero();
301 ConstQuaternionMap_t p0 (q0.derived().data());
302 ConstQuaternionMap_t p1 (q1.derived().data());
303 PINOCCHIO_EIGEN_CONST_CAST(Tangent_t,d)
304 =
log3((p0.matrix().transpose() * p1.matrix()).eval());
307 template <ArgumentPosition arg,
class ConfigL_t,
class ConfigR_t,
class JacobianOut_t>
308 void dDifference_impl (
const Eigen::MatrixBase<ConfigL_t> & q0,
309 const Eigen::MatrixBase<ConfigR_t> & q1,
310 const Eigen::MatrixBase<JacobianOut_t> & J)
const 312 ConstQuaternionMap_t p0 (q0.derived().data());
313 ConstQuaternionMap_t p1 (q1.derived().data());
314 Eigen::Matrix<Scalar, 3, 3> R = p0.matrix().transpose() * p1.matrix();
320 PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J).noalias() = - J1 * R.transpose();
321 }
else if (arg == ARG1) {
326 template <
class ConfigIn_t,
class Velocity_t,
class ConfigOut_t>
327 static void integrate_impl(
const Eigen::MatrixBase<ConfigIn_t> & q,
328 const Eigen::MatrixBase<Velocity_t> & v,
329 const Eigen::MatrixBase<ConfigOut_t> & qout)
331 ConstQuaternionMap_t quat(q.derived().data());
332 QuaternionMap_t quat_map(PINOCCHIO_EIGEN_CONST_CAST(ConfigOut_t,qout).data());
335 quat_map = quat * pOmega;
339 template <
class Config_t,
class Jacobian_t>
340 static void integrateCoeffWiseJacobian_impl(
const Eigen::MatrixBase<Config_t> & q,
341 const Eigen::MatrixBase<Jacobian_t> & J)
343 assert(J.rows() ==
nq() && J.cols() ==
nv() &&
"J is not of the right dimension");
345 typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(Config_t) ConfigPlainType;
346 typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(Jacobian_t) JacobianPlainType;
347 typedef typename ConfigPlainType::Scalar Scalar;
349 typedef typename SE3::Vector3 Vector3;
350 typedef typename SE3::Matrix3 Matrix3;
352 ConstQuaternionMap_t quat_map(q.derived().data());
353 Eigen::Matrix<Scalar,NQ,NV,JacobianPlainType::Options|Eigen::RowMajor> Jexp3QuatCoeffWise;
362 if(quat_map.coeffs()[3] >= 0.)
363 PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t,J).noalias() = Jexp3QuatCoeffWise * Jlog;
365 PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t,J).noalias() = -Jexp3QuatCoeffWise * Jlog;
370 template <
class Config_t,
class Tangent_t,
class JacobianOut_t>
371 static void dIntegrate_dq_impl(
const Eigen::MatrixBase<Config_t > & ,
372 const Eigen::MatrixBase<Tangent_t> & v,
373 const Eigen::MatrixBase<JacobianOut_t> & J)
375 JacobianOut_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J);
379 template <
class Config_t,
class Tangent_t,
class JacobianOut_t>
380 static void dIntegrate_dv_impl(
const Eigen::MatrixBase<Config_t > & ,
381 const Eigen::MatrixBase<Tangent_t> & v,
382 const Eigen::MatrixBase<JacobianOut_t> & J)
384 Jexp3(v, J.derived());
387 template <
class ConfigL_t,
class ConfigR_t,
class ConfigOut_t>
388 static void interpolate_impl(
const Eigen::MatrixBase<ConfigL_t> & q0,
389 const Eigen::MatrixBase<ConfigR_t> & q1,
391 const Eigen::MatrixBase<ConfigOut_t> & qout)
393 ConstQuaternionMap_t p0 (q0.derived().data());
394 ConstQuaternionMap_t p1 (q1.derived().data());
395 QuaternionMap_t quat_map(PINOCCHIO_EIGEN_CONST_CAST(ConfigOut_t,qout).data());
397 quat_map = p0.slerp(u, p1);
400 template <
class ConfigL_t,
class ConfigR_t>
401 static Scalar squaredDistance_impl(
const Eigen::MatrixBase<ConfigL_t> & q0,
402 const Eigen::MatrixBase<ConfigR_t> & q1)
405 difference_impl(q0, q1, t);
406 return t.squaredNorm();
409 template <
class Config_t>
410 static void normalize_impl(
const Eigen::MatrixBase<Config_t> & qout)
412 Config_t & qout_ = PINOCCHIO_EIGEN_CONST_CAST(Config_t,qout);
416 template <
class Config_t>
417 void random_impl(
const Eigen::MatrixBase<Config_t> & qout)
const 419 QuaternionMap_t quat_map(PINOCCHIO_EIGEN_CONST_CAST(Config_t,qout).data());
423 template <
class ConfigL_t,
class ConfigR_t,
class ConfigOut_t>
424 void randomConfiguration_impl(
const Eigen::MatrixBase<ConfigL_t> &,
425 const Eigen::MatrixBase<ConfigR_t> &,
426 const Eigen::MatrixBase<ConfigOut_t> & qout)
432 template <
class ConfigL_t,
class ConfigR_t>
433 static bool isSameConfiguration_impl(
const Eigen::MatrixBase<ConfigL_t> & q0,
434 const Eigen::MatrixBase<ConfigR_t> & q1,
437 ConstQuaternionMap_t quat1(q0.derived().data());
438 ConstQuaternionMap_t quat2(q1.derived().data());
446 #endif // ifndef __pinocchio_special_orthogonal_operation_hpp__ int nv(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointNvVisitor to get the dimension of the joint tangent space...
Eigen::Matrix< typename Matrix3Like::Scalar, 3, 1, Matrix3Like ::Options > log3(const Eigen::MatrixBase< Matrix3Like > &R, typename Matrix3Like::Scalar &theta)
Same as log3.
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.
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 Vector3Like::Scalar, 3, 3, Vector3Like ::Options > exp3(const Eigen::MatrixBase< Vector3Like > &v)
Exp: so3 -> SO3.
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 exp3(const Eigen::MatrixBase< Vector3Like > &v, Eigen::QuaternionBase< QuaternionLike > &quat_out)
Exp: so3 -> SO3 (quaternion)
void Jexp3CoeffWise(const Eigen::MatrixBase< Vector3Like > &v, const Eigen::MatrixBase< Matrix43Like > &Jexp)
Derivative of where is a small perturbation of at identity.
void Jexp3(const Eigen::MatrixBase< Vector3Like > &r, const Eigen::MatrixBase< Matrix3Like > &Jexp)
Derivative of .
static Index nv()
Get dimension of Lie Group tangent space.
static Index nv()
Get dimension of Lie Group tangent space.
void SINCOS(const S1 &a, S2 *sa, S3 *ca)
Computes sin/cos values of a given input scalar.
void uniformRandom(const Eigen::QuaternionBase< Derived > &q)
Uniformly random quaternion sphere.
Main pinocchio namespace.
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.