pinocchio  2.1.3
special-orthogonal.hpp
1 //
2 // Copyright (c) 2016-2018 CNRS
3 //
4 
5 #ifndef __pinocchio_special_orthogonal_operation_hpp__
6 #define __pinocchio_special_orthogonal_operation_hpp__
7 
8 #include <limits>
9 
10 #include <pinocchio/spatial/explog.hpp>
11 #include <pinocchio/math/quaternion.hpp>
12 #include <pinocchio/multibody/liegroup/liegroup-base.hpp>
13 
14 namespace pinocchio
15 {
16  template<int Dim, typename Scalar, int Options = 0>
17  struct SpecialOrthogonalOperationTpl
18  {};
19 
20  template<int Dim, typename Scalar, int Options>
21  struct traits< SpecialOrthogonalOperationTpl<Dim,Scalar,Options> >
22  {};
23 
24  template<typename _Scalar, int _Options>
25  struct traits< SpecialOrthogonalOperationTpl<2,_Scalar,_Options> >
26  {
27  typedef _Scalar Scalar;
28  enum
29  {
30  Options = _Options,
31  NQ = 2,
32  NV = 1
33  };
34  };
35 
36  template<typename _Scalar, int _Options >
37  struct traits<SpecialOrthogonalOperationTpl<3,_Scalar,_Options> > {
38  typedef _Scalar Scalar;
39  enum
40  {
41  Options = _Options,
42  NQ = 4,
43  NV = 3
44  };
45  };
46 
47  template<typename _Scalar, int _Options>
48  struct SpecialOrthogonalOperationTpl<2,_Scalar,_Options>
49  : public LieGroupBase< SpecialOrthogonalOperationTpl<2,_Scalar,_Options> >
50  {
51  PINOCCHIO_LIE_GROUP_TPL_PUBLIC_INTERFACE(SpecialOrthogonalOperationTpl);
52  typedef Eigen::Matrix<Scalar,2,2> Matrix2;
53 
54  template<typename Matrix2Like>
55  static typename Matrix2Like::Scalar
56  log(const Eigen::MatrixBase<Matrix2Like> & R)
57  {
58 
59  typedef typename Matrix2Like::Scalar Scalar;
60  EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix2Like,2,2);
61 
62  Scalar theta;
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); // acos((3-1)/2)
67  else if (tr < Scalar(-2)) theta = (pos ? PI_value : -PI_value); // acos((-1-1)/2)
68  // Around 0, asin is numerically more stable than acos because
69  // acos(x) = PI/2 - x and asin(x) = x (the precision of x is not lost in PI/2).
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); // theta != NaN
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));
75  return theta;
76  }
77 
78  template<typename Matrix2Like>
79  static typename Matrix2Like::Scalar
80  Jlog(const Eigen::MatrixBase<Matrix2Like> &)
81  {
82  typedef typename Matrix2Like::Scalar Scalar;
83  EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix2Like,2,2);
84  return (Scalar)1;
85  }
86 
91  static Index nq()
92  {
93  return NQ;
94  }
95 
97  static Index nv()
98  {
99  return NV;
100  }
101 
102  static ConfigVector_t neutral()
103  {
104  ConfigVector_t n; n << Scalar(1), Scalar(0);
105  return n;
106  }
107 
108  static std::string name()
109  {
110  return std::string("SO(2)");
111  }
112 
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)
117  {
118  if (q0 == q1) {
119  PINOCCHIO_EIGEN_CONST_CAST(Tangent_t,d).setZero();
120  return;
121  }
122  Matrix2 R; // R0.transpose() * R1;
123  R(0,0) = R(1,1) = q0.dot(q1);
124  R(1,0) = q0(0) * q1(1) - q0(1) * q1(0);
125  R(0,1) = - R(1,0);
126  PINOCCHIO_EIGEN_CONST_CAST(Tangent_t,d)[0] = log(R);
127  }
128 
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
133  {
134  Matrix2 R; // R0.transpose() * R1;
135  R(0,0) = R(1,1) = q0.dot(q1);
136  R(1,0) = q0(0) * q1(1) - q0(1) * q1(0);
137  R(0,1) = - R(1,0);
138 
139  Scalar w (Jlog(R));
140  PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J).coeffRef(0,0) = ((arg==ARG0) ? -w : w);
141  }
142 
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)
147  {
148  ConfigOut_t & out = PINOCCHIO_EIGEN_CONST_CAST(ConfigOut_t,qout);
149 
150  const Scalar & ca = q(0);
151  const Scalar & sa = q(1);
152  const Scalar & omega = v(0);
153 
154  Scalar cosOmega,sinOmega; SINCOS(omega, &sinOmega, &cosOmega);
155  // TODO check the cost of atan2 vs SINCOS
156 
157  out << cosOmega * ca - sinOmega * sa,
158  sinOmega * ca + cosOmega * sa;
159  // First order approximation of the normalization of the unit complex
160  // See quaternion::firstOrderNormalize for equations.
161  const Scalar norm2 = out.squaredNorm();
162  out *= (3 - norm2) / 2;
163  }
164 
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)
168  {
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);
171  Jout << -q[1], q[0];
172  }
173 
174  template <class Config_t, class Tangent_t, class JacobianOut_t>
175  static void dIntegrate_dq_impl(const Eigen::MatrixBase<Config_t > & /*q*/,
176  const Eigen::MatrixBase<Tangent_t> & /*v*/,
177  const Eigen::MatrixBase<JacobianOut_t>& J)
178  {
179  JacobianOut_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J);
180  Jout(0,0) = 1;
181  }
182 
183  template <class Config_t, class Tangent_t, class JacobianOut_t>
184  static void dIntegrate_dv_impl(const Eigen::MatrixBase<Config_t > & /*q*/,
185  const Eigen::MatrixBase<Tangent_t> & /*v*/,
186  const Eigen::MatrixBase<JacobianOut_t>& J)
187  {
188  JacobianOut_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J);
189  Jout(0,0) = 1;
190  }
191 
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,
195  const Scalar& u,
196  const Eigen::MatrixBase<ConfigOut_t>& qout)
197  {
198  ConfigOut_t & out = PINOCCHIO_EIGEN_CONST_CAST(ConfigOut_t,qout);
199 
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);
206 
207  const Scalar PI_value = PI<Scalar>();
208 
209  if (fabs (theta) > 1e-6 && fabs (theta) < PI_value - 1e-6)
210  {
211  out = (sin ((1-u)*theta)/sinTheta) * q0
212  + (sin ( u *theta)/sinTheta) * q1;
213  }
214  else if (fabs (theta) < 1e-6) // theta = 0
215  {
216  out = (1-u) * q0 + u * q1;
217  }
218  else // theta = +-PI
219  {
220  Scalar theta0 = atan2 (q0(1), q0(0));
221  SINCOS(theta0,&out[1],&out[0]);
222  }
223  }
224 
225  // template <class ConfigL_t, class ConfigR_t>
226  // static double squaredDistance_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
227  // const Eigen::MatrixBase<ConfigR_t> & q1)
228 
229  template <class Config_t>
230  static void normalize_impl (const Eigen::MatrixBase<Config_t>& qout)
231  {
232  Config_t& qout_ = (const_cast< Eigen::MatrixBase<Config_t>& >(qout)).derived();
233  qout_.normalize();
234  }
235 
236  template <class Config_t>
237  void random_impl (const Eigen::MatrixBase<Config_t>& qout) const
238  {
239  Config_t & out = PINOCCHIO_EIGEN_CONST_CAST(Config_t,qout);
240 
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));
244  }
245 
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)
250  const
251  {
252  random_impl(qout);
253  }
254  }; // struct SpecialOrthogonalOperationTpl<2,_Scalar,_Options>
255 
256  template<typename _Scalar, int _Options>
257  struct SpecialOrthogonalOperationTpl<3,_Scalar,_Options>
258  : public LieGroupBase< SpecialOrthogonalOperationTpl<3,_Scalar,_Options> >
259  {
260  PINOCCHIO_LIE_GROUP_TPL_PUBLIC_INTERFACE(SpecialOrthogonalOperationTpl);
261 
262  typedef Eigen::Quaternion<Scalar> Quaternion_t;
263  typedef Eigen::Map< Quaternion_t> QuaternionMap_t;
264  typedef Eigen::Map<const Quaternion_t> ConstQuaternionMap_t;
265 
270  static Index nq()
271  {
272  return NQ;
273  }
275  static Index nv()
276  {
277  return NV;
278  }
279 
280  static ConfigVector_t neutral()
281  {
282  ConfigVector_t n; n.setZero (); n[3] = Scalar(1);
283  return n;
284  }
285 
286  static std::string name()
287  {
288  return std::string("SO(3)");
289  }
290 
291  template <class ConfigL_t, class ConfigR_t, class Tangent_t>
292  static void difference_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
293  const Eigen::MatrixBase<ConfigR_t> & q1,
294  const Eigen::MatrixBase<Tangent_t> & d)
295  {
296  if (q0 == q1) {
297  PINOCCHIO_EIGEN_CONST_CAST(Tangent_t,d).setZero();
298  return;
299  }
300  ConstQuaternionMap_t p0 (q0.derived().data());
301  ConstQuaternionMap_t p1 (q1.derived().data());
302  PINOCCHIO_EIGEN_CONST_CAST(Tangent_t,d)
303  = log3((p0.matrix().transpose() * p1.matrix()).eval());
304  }
305 
306  template <ArgumentPosition arg, class ConfigL_t, class ConfigR_t, class JacobianOut_t>
307  void dDifference_impl (const Eigen::MatrixBase<ConfigL_t> & q0,
308  const Eigen::MatrixBase<ConfigR_t> & q1,
309  const Eigen::MatrixBase<JacobianOut_t>& J) const
310  {
311  ConstQuaternionMap_t p0 (q0.derived().data());
312  ConstQuaternionMap_t p1 (q1.derived().data());
313  Eigen::Matrix<Scalar, 3, 3> R = p0.matrix().transpose() * p1.matrix();
314 
315  if (arg == ARG0) {
316  JacobianMatrix_t J1;
317  Jlog3 (R, J1);
318 
319  PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J).noalias() = - J1 * R.transpose();
320  } else if (arg == ARG1) {
321  Jlog3 (R, J);
322  }
323  }
324 
325  template <class ConfigIn_t, class Velocity_t, class ConfigOut_t>
326  static void integrate_impl(const Eigen::MatrixBase<ConfigIn_t> & q,
327  const Eigen::MatrixBase<Velocity_t> & v,
328  const Eigen::MatrixBase<ConfigOut_t> & qout)
329  {
330  ConstQuaternionMap_t quat(q.derived().data());
331  QuaternionMap_t quat_map(PINOCCHIO_EIGEN_CONST_CAST(ConfigOut_t,qout).data());
332 
333  Quaternion_t pOmega; quaternion::exp3(v,pOmega);
334  quat_map = quat * pOmega;
335  quaternion::firstOrderNormalize(quat_map);
336  }
337 
338  template <class Config_t, class Jacobian_t>
339  static void integrateCoeffWiseJacobian_impl(const Eigen::MatrixBase<Config_t> & q,
340  const Eigen::MatrixBase<Jacobian_t> & J)
341  {
342  assert(J.rows() == nq() && J.cols() == nv() && "J is not of the right dimension");
343 
344  typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(Config_t) ConfigPlainType;
345  typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(Jacobian_t) JacobianPlainType;
346  typedef typename ConfigPlainType::Scalar Scalar;
347  typedef SE3Tpl<Scalar,ConfigPlainType::Options> SE3;
348  typedef typename SE3::Vector3 Vector3;
349  typedef typename SE3::Matrix3 Matrix3;
350 
351  ConstQuaternionMap_t quat_map(q.derived().data());
352  Eigen::Matrix<Scalar,NQ,NV,JacobianPlainType::Options|Eigen::RowMajor> Jexp3QuatCoeffWise;
353 
354  Scalar theta;
355  Vector3 v = quaternion::log3(quat_map,theta);
356  quaternion::Jexp3CoeffWise(v,Jexp3QuatCoeffWise);
357  Matrix3 Jlog;
358  Jlog3(theta,v,Jlog);
359 
360 // if(quat_map.w() >= 0.) // comes from the log3 for quaternions which may change the sign.
361  if(quat_map.coeffs()[3] >= 0.) // comes from the log3 for quaternions which may change the sign.
362  PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t,J).noalias() = Jexp3QuatCoeffWise * Jlog;
363  else
364  PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t,J).noalias() = -Jexp3QuatCoeffWise * Jlog;
365 
366 // Jexp3(quat_map,PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t,J).template topLeftCorner<NQ,NV>());
367  }
368 
369  template <class Config_t, class Tangent_t, class JacobianOut_t>
370  static void dIntegrate_dq_impl(const Eigen::MatrixBase<Config_t > & /*q*/,
371  const Eigen::MatrixBase<Tangent_t> & v,
372  const Eigen::MatrixBase<JacobianOut_t>& J)
373  {
374  JacobianOut_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J);
375  Jout = exp3(-v);
376  }
377 
378  template <class Config_t, class Tangent_t, class JacobianOut_t>
379  static void dIntegrate_dv_impl(const Eigen::MatrixBase<Config_t > & /*q*/,
380  const Eigen::MatrixBase<Tangent_t> & v,
381  const Eigen::MatrixBase<JacobianOut_t> & J)
382  {
383  Jexp3(v, J.derived());
384  }
385 
386  template <class ConfigL_t, class ConfigR_t, class ConfigOut_t>
387  static void interpolate_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
388  const Eigen::MatrixBase<ConfigR_t> & q1,
389  const Scalar & u,
390  const Eigen::MatrixBase<ConfigOut_t>& qout)
391  {
392  ConstQuaternionMap_t p0 (q0.derived().data());
393  ConstQuaternionMap_t p1 (q1.derived().data());
394  QuaternionMap_t quat_map(PINOCCHIO_EIGEN_CONST_CAST(ConfigOut_t,qout).data());
395 
396  quat_map = p0.slerp(u, p1);
397  }
398 
399  template <class ConfigL_t, class ConfigR_t>
400  static Scalar squaredDistance_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
401  const Eigen::MatrixBase<ConfigR_t> & q1)
402  {
403  TangentVector_t t;
404  difference_impl(q0, q1, t);
405  return t.squaredNorm();
406  }
407 
408  template <class Config_t>
409  static void normalize_impl(const Eigen::MatrixBase<Config_t>& qout)
410  {
411  Config_t & qout_ = PINOCCHIO_EIGEN_CONST_CAST(Config_t,qout);
412  qout_.normalize();
413  }
414 
415  template <class Config_t>
416  void random_impl(const Eigen::MatrixBase<Config_t> & qout) const
417  {
418  QuaternionMap_t quat_map(PINOCCHIO_EIGEN_CONST_CAST(Config_t,qout).data());
419  quaternion::uniformRandom(quat_map);
420  }
421 
422  template <class ConfigL_t, class ConfigR_t, class ConfigOut_t>
423  void randomConfiguration_impl(const Eigen::MatrixBase<ConfigL_t> &,
424  const Eigen::MatrixBase<ConfigR_t> &,
425  const Eigen::MatrixBase<ConfigOut_t> & qout)
426  const
427  {
428  random_impl(qout);
429  }
430 
431  template <class ConfigL_t, class ConfigR_t>
432  static bool isSameConfiguration_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
433  const Eigen::MatrixBase<ConfigR_t> & q1,
434  const Scalar & prec)
435  {
436  ConstQuaternionMap_t quat1(q0.derived().data());
437  ConstQuaternionMap_t quat2(q1.derived().data());
438 
439  return quaternion::defineSameRotation(quat1,quat2,prec);
440  }
441  }; // struct SpecialOrthogonalOperationTpl<3,_Scalar,_Options>
442 
443 } // namespace pinocchio
444 
445 #endif // ifndef __pinocchio_special_orthogonal_operation_hpp__
pinocchio::nv
int nv(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointNvVisitor to get the dimension of the joint tangent space...
pinocchio::log3
Eigen::Matrix< typename Matrix3Like::Scalar, 3, 1, Matrix3Like::Options > log3(const Eigen::MatrixBase< Matrix3Like > &R, typename Matrix3Like::Scalar &theta)
Same as log3.
Definition: explog.hpp:78
pinocchio::quaternion::defineSameRotation
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.
Definition: quaternion.hpp:48
pinocchio::nq
int nq(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointNqVisitor to get the dimension of the joint configuration space...
pinocchio::exp3
Eigen::Matrix< typename Vector3Like::Scalar, 3, 3, Vector3Like::Options > exp3(const Eigen::MatrixBase< Vector3Like > &v)
Exp: so3 -> SO3.
Definition: explog.hpp:31
pinocchio::quaternion::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.
Definition: explog-quaternion.hpp:75
pinocchio::name
std::string name(const LieGroupGenericTpl< LieGroupCollection > &lg)
Visit a LieGroupVariant to get the name of it.
pinocchio::quaternion::firstOrderNormalize
void firstOrderNormalize(const Eigen::QuaternionBase< D > &q)
Definition: quaternion.hpp:77
pinocchio::quaternion::exp3
void exp3(const Eigen::MatrixBase< Vector3Like > &v, Eigen::QuaternionBase< QuaternionLike > &quat_out)
Exp: so3 -> SO3 (quaternion)
Definition: explog-quaternion.hpp:24
pinocchio::SINCOS
void SINCOS(const Scalar &a, Scalar *sa, Scalar *ca)
Computes sin/cos values of a given input scalar.
Definition: sincos.hpp:28
pinocchio::quaternion::Jexp3CoeffWise
void Jexp3CoeffWise(const Eigen::MatrixBase< Vector3Like > &v, const Eigen::MatrixBase< Matrix43Like > &Jexp)
Derivative of where is a small perturbation of at identity.
Definition: explog-quaternion.hpp:137
pinocchio::Jexp3
void Jexp3(const Eigen::MatrixBase< Vector3Like > &r, const Eigen::MatrixBase< Matrix3Like > &Jexp)
Derivative of .
Definition: explog.hpp:149
pinocchio::SpecialOrthogonalOperationTpl< 3, _Scalar, _Options >::nv
static Index nv()
Get dimension of Lie Group tangent space.
Definition: special-orthogonal.hpp:275
pinocchio::SpecialOrthogonalOperationTpl< 2, _Scalar, _Options >::nv
static Index nv()
Get dimension of Lie Group tangent space.
Definition: special-orthogonal.hpp:97
pinocchio::quaternion::uniformRandom
void uniformRandom(const Eigen::QuaternionBase< Derived > &q)
Uniformly random quaternion sphere.
Definition: quaternion.hpp:96
pinocchio
Main pinocchio namespace.
Definition: treeview.dox:24
pinocchio::SpecialOrthogonalOperationTpl< 2, _Scalar, _Options >::nq
static Index nq()
Definition: special-orthogonal.hpp:91
pinocchio::neutral
void neutral(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ReturnType > &qout)
Return the neutral configuration element related to the model configuration space.
Definition: joint-configuration.hpp:224
pinocchio::SpecialOrthogonalOperationTpl< 3, _Scalar, _Options >::nq
static Index nq()
Definition: special-orthogonal.hpp:270
pinocchio::traits
Common traits structure to fully define base classes for CRTP.
Definition: spatial/fwd.hpp:29