pinocchio  3.3.0
A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
special-orthogonal.hpp
1 //
2 // Copyright (c) 2016-2021 CNRS INRIA
3 //
4 
5 #ifndef __pinocchio_multibody_liegroup_special_orthogonal_operation_hpp__
6 #define __pinocchio_multibody_liegroup_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 #include "pinocchio/utils/static-if.hpp"
14 
15 namespace pinocchio
16 {
17  template<int Dim, typename Scalar, int Options = 0>
18  struct SpecialOrthogonalOperationTpl
19  {
20  };
21 
22  template<int Dim, typename Scalar, int Options>
23  struct traits<SpecialOrthogonalOperationTpl<Dim, Scalar, Options>>
24  {
25  };
26 
27  template<typename _Scalar, int _Options>
28  struct traits<SpecialOrthogonalOperationTpl<2, _Scalar, _Options>>
29  {
30  typedef _Scalar Scalar;
31  enum
32  {
33  Options = _Options,
34  NQ = 2,
35  NV = 1
36  };
37  };
38 
39  template<typename _Scalar, int _Options>
40  struct traits<SpecialOrthogonalOperationTpl<3, _Scalar, _Options>>
41  {
42  typedef _Scalar Scalar;
43  enum
44  {
45  Options = _Options,
46  NQ = 4,
47  NV = 3
48  };
49  };
50 
51  template<typename _Scalar, int _Options>
52  struct SpecialOrthogonalOperationTpl<2, _Scalar, _Options>
53  : public LieGroupBase<SpecialOrthogonalOperationTpl<2, _Scalar, _Options>>
54  {
55  PINOCCHIO_LIE_GROUP_TPL_PUBLIC_INTERFACE(SpecialOrthogonalOperationTpl);
56  typedef Eigen::Matrix<Scalar, 2, 2> Matrix2;
57  typedef typename Eigen::NumTraits<Scalar>::Real RealScalar;
58 
59  template<typename Matrix2Like>
60  static typename Matrix2Like::Scalar log(const Eigen::MatrixBase<Matrix2Like> & R)
61  {
62  typedef typename Matrix2Like::Scalar Scalar;
63  EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix2Like, 2, 2);
64 
65  const Scalar tr = R.trace();
66 
67  static const Scalar PI_value = PI<Scalar>();
68 
69  using internal::if_then_else;
70  Scalar theta = if_then_else(
71  internal::GT, tr, Scalar(2),
72  Scalar(0), // then
73  if_then_else(
74  internal::LT, tr, Scalar(-2),
75  if_then_else(
76  internal::GE, R(1, 0), Scalar(0), PI_value,
77  static_cast<Scalar>(-PI_value)), // then
78  if_then_else(
79  internal::GT, tr,
80  static_cast<Scalar>(Scalar(2) - Scalar(1e-2)), // TODO: change value
81  static_cast<Scalar>(asin((R(1, 0) - R(0, 1)) / Scalar(2))), // then
82  if_then_else(
83  internal::GE, R(1, 0), Scalar(0),
84  static_cast<Scalar>(acos(tr / Scalar(2))), // then
85  static_cast<Scalar>(-acos(tr / Scalar(2)))))));
86 
87  // const bool pos = (R (1, 0) > Scalar(0));
88  // if (tr > Scalar(2)) theta = Scalar(0); // acos((3-1)/2)
89  // else if (tr < Scalar(-2)) theta = (pos ? PI_value : -PI_value); // acos((-1-1)/2)
90  // Around 0, asin is numerically more stable than acos because
91  // acos(x) = PI/2 - x and asin(x) = x (the precision of x is not lost in PI/2).
92  // else if (tr > Scalar(2) - 1e-2) theta = asin ((R(1,0) - R(0,1)) / Scalar(2));
93  // else theta = (pos ? acos (tr/Scalar(2)) : -acos(tr/Scalar(2)));
94  assert(check_expression_if_real<Scalar>(theta == theta) && "theta is NaN"); // theta != NaN
95  // assert ((cos (theta) * R(0,0) + sin (theta) * R(1,0) > 0) &&
96  // (cos (theta) * R(1,0) - sin (theta) * R(0,0) < 1e-6));
97  return theta;
98  }
99 
100  template<typename Matrix2Like>
101  static typename Matrix2Like::Scalar Jlog(const Eigen::MatrixBase<Matrix2Like> &)
102  {
103  typedef typename Matrix2Like::Scalar Scalar;
104  EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix2Like, 2, 2);
105  return Scalar(1);
106  }
107 
112  static Index nq()
113  {
114  return NQ;
115  }
116 
118  static Index nv()
119  {
120  return NV;
121  }
122 
123  static ConfigVector_t neutral()
124  {
125  ConfigVector_t n;
126  n << Scalar(1), Scalar(0);
127  return n;
128  }
129 
130  static std::string name()
131  {
132  return std::string("SO(2)");
133  }
134 
135  template<class ConfigL_t, class ConfigR_t, class Tangent_t>
136  static void difference_impl(
137  const Eigen::MatrixBase<ConfigL_t> & q0,
138  const Eigen::MatrixBase<ConfigR_t> & q1,
139  const Eigen::MatrixBase<Tangent_t> & d)
140  {
141  Matrix2 R; // R0.transpose() * R1;
142  R(0, 0) = R(1, 1) = q0.dot(q1);
143  R(1, 0) = q0(0) * q1(1) - q0(1) * q1(0);
144  R(0, 1) = -R(1, 0);
145  PINOCCHIO_EIGEN_CONST_CAST(Tangent_t, d)[0] = log(R);
146  }
147 
148  template<ArgumentPosition arg, class ConfigL_t, class ConfigR_t, class JacobianOut_t>
149  static void dDifference_impl(
150  const Eigen::MatrixBase<ConfigL_t> & q0,
151  const Eigen::MatrixBase<ConfigR_t> & q1,
152  const Eigen::MatrixBase<JacobianOut_t> & J)
153  {
154  Matrix2 R; // R0.transpose() * R1;
155  R(0, 0) = R(1, 1) = q0.dot(q1);
156  R(1, 0) = q0(0) * q1(1) - q0(1) * q1(0);
157  R(0, 1) = -R(1, 0);
158 
159  Scalar w(Jlog(R));
160  PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, J).coeffRef(0, 0) = ((arg == ARG0) ? -w : w);
161  }
162 
163  template<class ConfigIn_t, class Velocity_t, class ConfigOut_t>
164  static void integrate_impl(
165  const Eigen::MatrixBase<ConfigIn_t> & q,
166  const Eigen::MatrixBase<Velocity_t> & v,
167  const Eigen::MatrixBase<ConfigOut_t> & qout)
168  {
169  ConfigOut_t & out = PINOCCHIO_EIGEN_CONST_CAST(ConfigOut_t, qout);
170 
171  const Scalar ca = q(0);
172  const Scalar sa = q(1);
173  const Scalar & omega = v(0);
174 
175  Scalar cosOmega, sinOmega;
176  SINCOS(omega, &sinOmega, &cosOmega);
177  // TODO check the cost of atan2 vs SINCOS
178 
179  out << cosOmega * ca - sinOmega * sa, sinOmega * ca + cosOmega * sa;
180  // First order approximation of the normalization of the unit complex
181  // See quaternion::firstOrderNormalize for equations.
182  const Scalar norm2 = out.squaredNorm();
183  out *= (3 - norm2) / 2;
184  assert(pinocchio::isNormalized(out));
185  }
186 
187  template<class Config_t, class Jacobian_t>
188  static void integrateCoeffWiseJacobian_impl(
189  const Eigen::MatrixBase<Config_t> & q, const Eigen::MatrixBase<Jacobian_t> & J)
190  {
191  assert(J.rows() == nq() && J.cols() == nv() && "J is not of the right dimension");
192  Jacobian_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t, J);
193  Jout << -q[1], q[0];
194  }
195 
196  template<class Config_t, class Tangent_t, class JacobianOut_t>
197  static void dIntegrate_dq_impl(
198  const Eigen::MatrixBase<Config_t> & /*q*/,
199  const Eigen::MatrixBase<Tangent_t> & /*v*/,
200  const Eigen::MatrixBase<JacobianOut_t> & J,
201  const AssignmentOperatorType op = SETTO)
202  {
203  JacobianOut_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, J);
204  switch (op)
205  {
206  case SETTO:
207  Jout(0, 0) = Scalar(1);
208  break;
209  case ADDTO:
210  Jout(0, 0) += Scalar(1);
211  break;
212  case RMTO:
213  Jout(0, 0) -= Scalar(1);
214  break;
215  default:
216  assert(false && "Wrong Op requesed value");
217  break;
218  }
219  }
220 
221  template<class Config_t, class Tangent_t, class JacobianOut_t>
222  static void dIntegrate_dv_impl(
223  const Eigen::MatrixBase<Config_t> & /*q*/,
224  const Eigen::MatrixBase<Tangent_t> & /*v*/,
225  const Eigen::MatrixBase<JacobianOut_t> & J,
226  const AssignmentOperatorType op = SETTO)
227  {
228  JacobianOut_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, J);
229  switch (op)
230  {
231  case SETTO:
232  Jout(0, 0) = Scalar(1);
233  break;
234  case ADDTO:
235  Jout(0, 0) += Scalar(1);
236  break;
237  case RMTO:
238  Jout(0, 0) -= Scalar(1);
239  break;
240  default:
241  assert(false && "Wrong Op requesed value");
242  break;
243  }
244  }
245 
246  template<class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
247  static void dIntegrateTransport_dq_impl(
248  const Eigen::MatrixBase<Config_t> & /*q*/,
249  const Eigen::MatrixBase<Tangent_t> & /*v*/,
250  const Eigen::MatrixBase<JacobianIn_t> & Jin,
251  const Eigen::MatrixBase<JacobianOut_t> & Jout)
252  {
253  PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, Jout) = Jin;
254  }
255 
256  template<class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
257  static void dIntegrateTransport_dv_impl(
258  const Eigen::MatrixBase<Config_t> & /*q*/,
259  const Eigen::MatrixBase<Tangent_t> & /*v*/,
260  const Eigen::MatrixBase<JacobianIn_t> & Jin,
261  const Eigen::MatrixBase<JacobianOut_t> & Jout)
262  {
263  PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, Jout) = Jin;
264  }
265 
266  template<class Config_t, class Tangent_t, class Jacobian_t>
267  static void dIntegrateTransport_dq_impl(
268  const Eigen::MatrixBase<Config_t> & /*q*/,
269  const Eigen::MatrixBase<Tangent_t> & /*v*/,
270  const Eigen::MatrixBase<Jacobian_t> & /*J*/)
271  {
272  }
273 
274  template<class Config_t, class Tangent_t, class Jacobian_t>
275  static void dIntegrateTransport_dv_impl(
276  const Eigen::MatrixBase<Config_t> & /*q*/,
277  const Eigen::MatrixBase<Tangent_t> & /*v*/,
278  const Eigen::MatrixBase<Jacobian_t> & /*J*/)
279  {
280  }
281 
282  template<class ConfigL_t, class ConfigR_t, class ConfigOut_t>
283  static void interpolate_impl(
284  const Eigen::MatrixBase<ConfigL_t> & q0,
285  const Eigen::MatrixBase<ConfigR_t> & q1,
286  const Scalar & u,
287  const Eigen::MatrixBase<ConfigOut_t> & qout)
288  {
289  ConfigOut_t & out = PINOCCHIO_EIGEN_CONST_CAST(ConfigOut_t, qout);
290 
291  assert(
292  check_expression_if_real<Scalar>(abs(q0.norm() - 1) < 1e-8)
293  && "initial configuration not normalized");
294  assert(
295  check_expression_if_real<Scalar>(abs(q1.norm() - 1) < 1e-8)
296  && "final configuration not normalized");
297  const Scalar cosTheta = q0.dot(q1);
298  const Scalar sinTheta = q0(0) * q1(1) - q0(1) * q1(0);
299  const Scalar theta = atan2(sinTheta, cosTheta);
300  assert(check_expression_if_real<Scalar>(fabs(sin(theta) - sinTheta) < 1e-8));
301 
302  static const Scalar PI_value = PI<Scalar>();
303  static const Scalar PI_value_lower = PI_value - static_cast<Scalar>(1e-6);
304  using namespace internal;
305 
306  // const Scalar theta0 = atan2(q0(1), q0(0));
307  const Scalar abs_theta = fabs(theta);
308  out[0] = if_then_else(
309  LT, abs_theta, static_cast<Scalar>(1e-6),
310  static_cast<Scalar>((Scalar(1) - u) * q0[0] + u * q1[0]), // then
311  if_then_else(
312  LT, abs_theta, PI_value_lower, // else
313  static_cast<Scalar>(
314  (sin((Scalar(1) - u) * theta) / sinTheta) * q0[0]
315  + (sin(u * theta) / sinTheta) * q1[0]), // then
316  q0(0) // cos(theta0) // else
317  ));
318 
319  out[1] = if_then_else(
320  LT, abs_theta, static_cast<Scalar>(1e-6),
321  static_cast<Scalar>((Scalar(1) - u) * q0[1] + u * q1[1]), // then
322  if_then_else(
323  LT, abs_theta, PI_value_lower, // else
324  static_cast<Scalar>(
325  (sin((Scalar(1) - u) * theta) / sinTheta) * q0[1]
326  + (sin(u * theta) / sinTheta) * q1[1]), // then
327  q0(1) // sin(theta0) // else
328  ));
329  }
330 
331  template<class Config_t>
332  static void normalize_impl(const Eigen::MatrixBase<Config_t> & qout)
333  {
334  pinocchio::normalize(qout.const_cast_derived());
335  }
336 
337  template<class Config_t>
338  static bool isNormalized_impl(const Eigen::MatrixBase<Config_t> & qin, const Scalar & prec)
339  {
340  const Scalar norm = qin.norm();
341  using std::abs;
342  return abs(norm - Scalar(1.0)) < prec;
343  }
344 
345  template<class Config_t>
346  static void random_impl(const Eigen::MatrixBase<Config_t> & qout)
347  {
348  Config_t & out = PINOCCHIO_EIGEN_CONST_CAST(Config_t, qout);
349 
350  static const Scalar PI_value = PI<Scalar>();
351  const Scalar angle = -PI_value + Scalar(2) * PI_value * ((Scalar)rand()) / RAND_MAX;
352  SINCOS(angle, &out(1), &out(0));
353  }
354 
355  template<class ConfigL_t, class ConfigR_t, class ConfigOut_t>
356  static void randomConfiguration_impl(
357  const Eigen::MatrixBase<ConfigL_t> &,
358  const Eigen::MatrixBase<ConfigR_t> &,
359  const Eigen::MatrixBase<ConfigOut_t> & qout)
360  {
361  random_impl(qout);
362  }
363  }; // struct SpecialOrthogonalOperationTpl<2,_Scalar,_Options>
364 
365  template<typename _Scalar, int _Options>
366  struct SpecialOrthogonalOperationTpl<3, _Scalar, _Options>
367  : public LieGroupBase<SpecialOrthogonalOperationTpl<3, _Scalar, _Options>>
368  {
369  PINOCCHIO_LIE_GROUP_TPL_PUBLIC_INTERFACE(SpecialOrthogonalOperationTpl);
370 
371  typedef Eigen::Quaternion<Scalar> Quaternion_t;
372  typedef Eigen::Map<Quaternion_t> QuaternionMap_t;
373  typedef Eigen::Map<const Quaternion_t> ConstQuaternionMap_t;
374  typedef SE3Tpl<Scalar, Options> SE3;
375  typedef typename Eigen::NumTraits<Scalar>::Real RealScalar;
376 
381  static Index nq()
382  {
383  return NQ;
384  }
385 
387  static Index nv()
388  {
389  return NV;
390  }
391 
392  static ConfigVector_t neutral()
393  {
394  ConfigVector_t n;
395  n.setZero();
396  n[3] = Scalar(1);
397  return n;
398  }
399 
400  static std::string name()
401  {
402  return std::string("SO(3)");
403  }
404 
405  template<class ConfigL_t, class ConfigR_t, class Tangent_t>
406  static void difference_impl(
407  const Eigen::MatrixBase<ConfigL_t> & q0,
408  const Eigen::MatrixBase<ConfigR_t> & q1,
409  const Eigen::MatrixBase<Tangent_t> & d)
410  {
411  ConstQuaternionMap_t quat0(q0.derived().data());
412  assert(quaternion::isNormalized(
413  quat0, RealScalar(PINOCCHIO_DEFAULT_QUATERNION_NORM_TOLERANCE_VALUE)));
414  ConstQuaternionMap_t quat1(q1.derived().data());
415  assert(quaternion::isNormalized(
416  quat1, RealScalar(PINOCCHIO_DEFAULT_QUATERNION_NORM_TOLERANCE_VALUE)));
417 
418  PINOCCHIO_EIGEN_CONST_CAST(Tangent_t, d) =
419  quaternion::log3(Quaternion_t(quat0.conjugate() * quat1));
420  }
421 
422  template<ArgumentPosition arg, class ConfigL_t, class ConfigR_t, class JacobianOut_t>
423  static void dDifference_impl(
424  const Eigen::MatrixBase<ConfigL_t> & q0,
425  const Eigen::MatrixBase<ConfigR_t> & q1,
426  const Eigen::MatrixBase<JacobianOut_t> & J)
427  {
428  typedef typename SE3::Matrix3 Matrix3;
429 
430  ConstQuaternionMap_t quat0(q0.derived().data());
431  assert(quaternion::isNormalized(
432  quat0, RealScalar(PINOCCHIO_DEFAULT_QUATERNION_NORM_TOLERANCE_VALUE)));
433  ConstQuaternionMap_t quat1(q1.derived().data());
434  assert(quaternion::isNormalized(
435  quat1, RealScalar(PINOCCHIO_DEFAULT_QUATERNION_NORM_TOLERANCE_VALUE)));
436 
437  // TODO: check whether the merge with 2.6.9 is correct
438  const Quaternion_t q = quat0.conjugate() * quat1;
439  const Matrix3 R = q.matrix();
440 
441  if (arg == ARG0)
442  {
443  JacobianMatrix_t J1;
444  Jlog3(R, J1);
445 
446  PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, J).noalias() = -J1 * R.transpose();
447  }
448  else if (arg == ARG1)
449  {
450  Jlog3(R, J);
451  }
452  /*
453  const Quaternion_t quat_diff = quat0.conjugate() * quat1;
454 
455  if (arg == ARG0) {
456 PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
457 PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED
458  JacobianMatrix_t J1;
459  quaternion::Jlog3(quat_diff, J1);
460 PINOCCHIO_COMPILER_DIAGNOSTIC_POP
461  const Matrix3 R = (quat_diff).matrix();
462 
463  PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J).noalias() = - J1 * R.transpose();
464  } else if (arg == ARG1) {
465  quaternion::Jlog3(quat_diff, J);
466  }
467  */
468  }
469 
470  template<class ConfigIn_t, class Velocity_t, class ConfigOut_t>
471  static void integrate_impl(
472  const Eigen::MatrixBase<ConfigIn_t> & q,
473  const Eigen::MatrixBase<Velocity_t> & v,
474  const Eigen::MatrixBase<ConfigOut_t> & qout)
475  {
476  ConstQuaternionMap_t quat(q.derived().data());
477  assert(quaternion::isNormalized(
478  quat, RealScalar(PINOCCHIO_DEFAULT_QUATERNION_NORM_TOLERANCE_VALUE)));
479  QuaternionMap_t quat_map(PINOCCHIO_EIGEN_CONST_CAST(ConfigOut_t, qout).data());
480 
481  Quaternion_t pOmega;
482  quaternion::exp3(v, pOmega);
483  quat_map = quat * pOmega;
484  quaternion::firstOrderNormalize(quat_map);
485  assert(quaternion::isNormalized(
486  quat_map, RealScalar(PINOCCHIO_DEFAULT_QUATERNION_NORM_TOLERANCE_VALUE)));
487  }
488 
489  template<class Config_t, class Jacobian_t>
490  static void integrateCoeffWiseJacobian_impl(
491  const Eigen::MatrixBase<Config_t> & q, const Eigen::MatrixBase<Jacobian_t> & J)
492  {
493  assert(J.rows() == nq() && J.cols() == nv() && "J is not of the right dimension");
494 
495  typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(Jacobian_t) JacobianPlainType;
496  typedef typename SE3::Vector3 Vector3;
497  typedef typename SE3::Matrix3 Matrix3;
498 
499  ConstQuaternionMap_t quat_map(q.derived().data());
500  assert(quaternion::isNormalized(
501  quat_map, RealScalar(PINOCCHIO_DEFAULT_QUATERNION_NORM_TOLERANCE_VALUE)));
502 
503  PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
504  PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED
505  Eigen::Matrix<Scalar, NQ, NV, JacobianPlainType::Options | Eigen::RowMajor>
506  Jexp3QuatCoeffWise;
507 
508  Scalar theta;
509  const Vector3 v = quaternion::log3(quat_map, theta);
510  quaternion::Jexp3CoeffWise(v, Jexp3QuatCoeffWise);
511  Matrix3 Jlog;
512  Jlog3(theta, v, Jlog);
513  PINOCCHIO_COMPILER_DIAGNOSTIC_POP
514 
515  // if(quat_map.w() >= 0.) // comes from the log3 for quaternions which may change the
516  // sign.
517  if (quat_map.coeffs()[3] >= 0.) // comes from the log3 for quaternions which may change the
518  // sign.
519  PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t, J).noalias() = Jexp3QuatCoeffWise * Jlog;
520  else
521  PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t, J).noalias() = -Jexp3QuatCoeffWise * Jlog;
522 
523  // Jexp3(quat_map,PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t,J).template
524  // topLeftCorner<NQ,NV>());
525  }
526 
527  template<class Config_t, class Tangent_t, class JacobianOut_t>
528  static void dIntegrate_dq_impl(
529  const Eigen::MatrixBase<Config_t> & /*q*/,
530  const Eigen::MatrixBase<Tangent_t> & v,
531  const Eigen::MatrixBase<JacobianOut_t> & J,
532  const AssignmentOperatorType op = SETTO)
533  {
534  JacobianOut_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, J);
535  switch (op)
536  {
537  case SETTO:
538  Jout = exp3(-v);
539  break;
540  case ADDTO:
541  Jout += exp3(-v);
542  break;
543  case RMTO:
544  Jout -= exp3(-v);
545  break;
546  default:
547  assert(false && "Wrong Op requesed value");
548  break;
549  }
550  }
551 
552  template<class Config_t, class Tangent_t, class JacobianOut_t>
553  static void dIntegrate_dv_impl(
554  const Eigen::MatrixBase<Config_t> & /*q*/,
555  const Eigen::MatrixBase<Tangent_t> & v,
556  const Eigen::MatrixBase<JacobianOut_t> & J,
557  const AssignmentOperatorType op = SETTO)
558  {
559  switch (op)
560  {
561  case SETTO:
562  Jexp3<SETTO>(v, J.derived());
563  break;
564  case ADDTO:
565  Jexp3<ADDTO>(v, J.derived());
566  break;
567  case RMTO:
568  Jexp3<RMTO>(v, J.derived());
569  break;
570  default:
571  assert(false && "Wrong Op requesed value");
572  break;
573  }
574  }
575 
576  template<class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
577  static void dIntegrateTransport_dq_impl(
578  const Eigen::MatrixBase<Config_t> & /*q*/,
579  const Eigen::MatrixBase<Tangent_t> & v,
580  const Eigen::MatrixBase<JacobianIn_t> & Jin,
581  const Eigen::MatrixBase<JacobianOut_t> & J_out)
582  {
583  typedef typename SE3::Matrix3 Matrix3;
584  JacobianOut_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, J_out);
585  const Matrix3 Jtmp3 = exp3(-v);
586  Jout.noalias() = Jtmp3 * Jin;
587  }
588 
589  template<class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
590  static void dIntegrateTransport_dv_impl(
591  const Eigen::MatrixBase<Config_t> & /*q*/,
592  const Eigen::MatrixBase<Tangent_t> & v,
593  const Eigen::MatrixBase<JacobianIn_t> & Jin,
594  const Eigen::MatrixBase<JacobianOut_t> & J_out)
595  {
596  typedef typename SE3::Matrix3 Matrix3;
597  JacobianOut_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, J_out);
598  PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
599  PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED
600  Matrix3 Jtmp3;
601  Jexp3<SETTO>(v, Jtmp3);
602  PINOCCHIO_COMPILER_DIAGNOSTIC_POP
603  Jout.noalias() = Jtmp3 * Jin;
604  }
605 
606  template<class Config_t, class Tangent_t, class Jacobian_t>
607  static void dIntegrateTransport_dq_impl(
608  const Eigen::MatrixBase<Config_t> & /*q*/,
609  const Eigen::MatrixBase<Tangent_t> & v,
610  const Eigen::MatrixBase<Jacobian_t> & J_out)
611  {
612  typedef typename SE3::Matrix3 Matrix3;
613  Jacobian_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t, J_out);
614  const Matrix3 Jtmp3 = exp3(-v);
615  Jout = Jtmp3 * Jout;
616  }
617 
618  template<class Config_t, class Tangent_t, class Jacobian_t>
619  static void dIntegrateTransport_dv_impl(
620  const Eigen::MatrixBase<Config_t> & /*q*/,
621  const Eigen::MatrixBase<Tangent_t> & v,
622  const Eigen::MatrixBase<Jacobian_t> & J_out)
623  {
624  typedef typename SE3::Matrix3 Matrix3;
625  Jacobian_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t, J_out);
626  PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
627  PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED
628  Matrix3 Jtmp3;
629  Jexp3<SETTO>(v, Jtmp3);
630  PINOCCHIO_COMPILER_DIAGNOSTIC_POP
631  Jout = Jtmp3 * Jout;
632  }
633 
634  template<class ConfigL_t, class ConfigR_t, class ConfigOut_t>
635  static void interpolate_impl(
636  const Eigen::MatrixBase<ConfigL_t> & q0,
637  const Eigen::MatrixBase<ConfigR_t> & q1,
638  const Scalar & u,
639  const Eigen::MatrixBase<ConfigOut_t> & qout)
640  {
641  ConstQuaternionMap_t quat0(q0.derived().data());
642  assert(quaternion::isNormalized(
643  quat0, RealScalar(PINOCCHIO_DEFAULT_QUATERNION_NORM_TOLERANCE_VALUE)));
644  ConstQuaternionMap_t quat1(q1.derived().data());
645  assert(quaternion::isNormalized(
646  quat1, RealScalar(PINOCCHIO_DEFAULT_QUATERNION_NORM_TOLERANCE_VALUE)));
647 
648  QuaternionMap_t quat_res(PINOCCHIO_EIGEN_CONST_CAST(ConfigOut_t, qout).data());
649 
650  TangentVector_t w;
651  difference_impl(q0, q1, w);
652  integrate_impl(q0, u * w, qout);
653  assert(quaternion::isNormalized(
654  quat_res, RealScalar(PINOCCHIO_DEFAULT_QUATERNION_NORM_TOLERANCE_VALUE)));
655  }
656 
657  template<class ConfigL_t, class ConfigR_t>
658  static Scalar squaredDistance_impl(
659  const Eigen::MatrixBase<ConfigL_t> & q0, const Eigen::MatrixBase<ConfigR_t> & q1)
660  {
661  PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
662  PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED
663  TangentVector_t t;
664  difference_impl(q0, q1, t);
665  PINOCCHIO_COMPILER_DIAGNOSTIC_POP
666  return t.squaredNorm();
667  }
668 
669  template<class Config_t>
670  static void normalize_impl(const Eigen::MatrixBase<Config_t> & qout)
671  {
672  pinocchio::normalize(qout.const_cast_derived());
673  }
674 
675  template<class Config_t>
676  static bool isNormalized_impl(const Eigen::MatrixBase<Config_t> & qin, const Scalar & prec)
677  {
678  const Scalar norm = qin.norm();
679  using std::abs;
680  return abs(norm - Scalar(1.0)) < prec;
681  }
682 
683  template<class Config_t>
684  static void random_impl(const Eigen::MatrixBase<Config_t> & qout)
685  {
686  QuaternionMap_t quat_map(PINOCCHIO_EIGEN_CONST_CAST(Config_t, qout).data());
687  quaternion::uniformRandom(quat_map);
688 
689  assert(quaternion::isNormalized(
690  quat_map, RealScalar(PINOCCHIO_DEFAULT_QUATERNION_NORM_TOLERANCE_VALUE)));
691  }
692 
693  template<class ConfigL_t, class ConfigR_t, class ConfigOut_t>
694  static void randomConfiguration_impl(
695  const Eigen::MatrixBase<ConfigL_t> &,
696  const Eigen::MatrixBase<ConfigR_t> &,
697  const Eigen::MatrixBase<ConfigOut_t> & qout)
698  {
699  random_impl(qout);
700  }
701 
702  template<class ConfigL_t, class ConfigR_t>
703  static bool isSameConfiguration_impl(
704  const Eigen::MatrixBase<ConfigL_t> & q0,
705  const Eigen::MatrixBase<ConfigR_t> & q1,
706  const Scalar & prec)
707  {
708  ConstQuaternionMap_t quat1(q0.derived().data());
709  assert(quaternion::isNormalized(
710  quat1, RealScalar(PINOCCHIO_DEFAULT_QUATERNION_NORM_TOLERANCE_VALUE)));
711  ConstQuaternionMap_t quat2(q1.derived().data());
712  assert(quaternion::isNormalized(
713  quat1, RealScalar(PINOCCHIO_DEFAULT_QUATERNION_NORM_TOLERANCE_VALUE)));
714 
715  return quaternion::defineSameRotation(quat1, quat2, prec);
716  }
717  }; // struct SpecialOrthogonalOperationTpl<3,_Scalar,_Options>
718 
719 } // namespace pinocchio
720 
721 #endif // ifndef __pinocchio_multibody_liegroup_special_orthogonal_operation_hpp__
pinocchio::quaternion::exp3
void exp3(const Eigen::MatrixBase< Vector3Like > &v, Eigen::QuaternionBase< QuaternionLike > &quat_out)
Exp: so3 -> SO3 (quaternion)
Definition: explog-quaternion.hpp:27
pinocchio::quaternion::firstOrderNormalize
void firstOrderNormalize(const Eigen::QuaternionBase< D > &q)
Definition: quaternion.hpp:89
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:57
pinocchio::quaternion::isNormalized
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.
Definition: quaternion.hpp:229
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:211
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:292
pinocchio::quaternion::uniformRandom
void uniformRandom(Eigen::QuaternionBase< Derived > &q)
Uniformly random quaternion sphere.
Definition: quaternion.hpp:114
pinocchio
Main pinocchio namespace.
Definition: treeview.dox:11
pinocchio::exp3
Eigen::Matrix< typename Vector3Like::Scalar, 3, 3, Vector3Like ::Options > exp3(const Eigen::MatrixBase< Vector3Like > &v)
Exp: so3 -> SO3.
Definition: explog.hpp:36
pinocchio::Jlog3
void Jlog3(const Scalar &theta, const Eigen::MatrixBase< Vector3Like > &log, const Eigen::MatrixBase< Matrix3Like > &Jlog)
Derivative of log3.
Definition: explog.hpp:240
pinocchio::isNormalized
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.
Definition: joint-configuration.hpp:933
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::nq
int nq(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointNqVisitor to get the dimension of the joint configuration space.
pinocchio::name
std::string name(const LieGroupGenericTpl< LieGroupCollection > &lg)
Visit a LieGroupVariant to get the name of it.
pinocchio::normalize
void normalize(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorType > &qout)
Normalize a configuration vector.
Definition: joint-configuration.hpp:887
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:363
pinocchio::SINCOS
void SINCOS(const S1 &a, S2 *sa, S3 *ca)
Computes sin/cos values of a given input scalar.
Definition: sincos.hpp:27
pinocchio::SpecialOrthogonalOperationTpl< 2, _Scalar, _Options >::nv
static Index nv()
Get dimension of Lie Group tangent space.
Definition: special-orthogonal.hpp:118
pinocchio::SpecialOrthogonalOperationTpl< 2, _Scalar, _Options >::nq
static Index nq()
Definition: special-orthogonal.hpp:112
pinocchio::SpecialOrthogonalOperationTpl< 3, _Scalar, _Options >::nv
static Index nv()
Get dimension of Lie Group tangent space.
Definition: special-orthogonal.hpp:387
pinocchio::SpecialOrthogonalOperationTpl< 3, _Scalar, _Options >::nq
static Index nq()
Definition: special-orthogonal.hpp:381
pinocchio::traits
Common traits structure to fully define base classes for CRTP.
Definition: fwd.hpp:72