pinocchio  3.5.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(quat0));
413  ConstQuaternionMap_t quat1(q1.derived().data());
414  assert(quaternion::isNormalized(quat1));
415 
416  PINOCCHIO_EIGEN_CONST_CAST(Tangent_t, d) =
417  quaternion::log3(Quaternion_t(quat0.conjugate() * quat1));
418  }
419 
420  template<ArgumentPosition arg, class ConfigL_t, class ConfigR_t, class JacobianOut_t>
421  static void dDifference_impl(
422  const Eigen::MatrixBase<ConfigL_t> & q0,
423  const Eigen::MatrixBase<ConfigR_t> & q1,
424  const Eigen::MatrixBase<JacobianOut_t> & J)
425  {
426  typedef typename SE3::Matrix3 Matrix3;
427 
428  ConstQuaternionMap_t quat0(q0.derived().data());
429  assert(quaternion::isNormalized(quat0));
430  ConstQuaternionMap_t quat1(q1.derived().data());
431  assert(quaternion::isNormalized(quat1));
432 
433  // TODO: check whether the merge with 2.6.9 is correct
434  const Quaternion_t q = quat0.conjugate() * quat1;
435  const Matrix3 R = q.matrix();
436 
437  if (arg == ARG0)
438  {
439  JacobianMatrix_t J1;
440  Jlog3(R, J1);
441 
442  PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, J).noalias() = -J1 * R.transpose();
443  }
444  else if (arg == ARG1)
445  {
446  Jlog3(R, J);
447  }
448  /*
449  const Quaternion_t quat_diff = quat0.conjugate() * quat1;
450 
451  if (arg == ARG0) {
452 PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
453 PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED
454  JacobianMatrix_t J1;
455  quaternion::Jlog3(quat_diff, J1);
456 PINOCCHIO_COMPILER_DIAGNOSTIC_POP
457  const Matrix3 R = (quat_diff).matrix();
458 
459  PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J).noalias() = - J1 * R.transpose();
460  } else if (arg == ARG1) {
461  quaternion::Jlog3(quat_diff, J);
462  }
463  */
464  }
465 
466  template<class ConfigIn_t, class Velocity_t, class ConfigOut_t>
467  static void integrate_impl(
468  const Eigen::MatrixBase<ConfigIn_t> & q,
469  const Eigen::MatrixBase<Velocity_t> & v,
470  const Eigen::MatrixBase<ConfigOut_t> & qout)
471  {
472  ConstQuaternionMap_t quat(q.derived().data());
473  assert(quaternion::isNormalized(quat));
474  QuaternionMap_t quat_map(PINOCCHIO_EIGEN_CONST_CAST(ConfigOut_t, qout).data());
475 
476  Quaternion_t pOmega;
477  quaternion::exp3(v, pOmega);
478  quat_map = quat * pOmega;
479  quaternion::firstOrderNormalize(quat_map);
480  assert(quaternion::isNormalized(quat_map));
481  }
482 
483  template<class Config_t, class Jacobian_t>
484  static void integrateCoeffWiseJacobian_impl(
485  const Eigen::MatrixBase<Config_t> & q, const Eigen::MatrixBase<Jacobian_t> & J)
486  {
487  assert(J.rows() == nq() && J.cols() == nv() && "J is not of the right dimension");
488 
489  typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(Jacobian_t) JacobianPlainType;
490  typedef typename SE3::Vector3 Vector3;
491  typedef typename SE3::Matrix3 Matrix3;
492 
493  ConstQuaternionMap_t quat_map(q.derived().data());
494  assert(quaternion::isNormalized(quat_map));
495 
496  PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
497  PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED
498  Eigen::Matrix<Scalar, NQ, NV, JacobianPlainType::Options | Eigen::RowMajor>
499  Jexp3QuatCoeffWise;
500 
501  Scalar theta;
502  const Vector3 v = quaternion::log3(quat_map, theta);
503  quaternion::Jexp3CoeffWise(v, Jexp3QuatCoeffWise);
504  Matrix3 Jlog;
505  Jlog3(theta, v, Jlog);
506  PINOCCHIO_COMPILER_DIAGNOSTIC_POP
507 
508  // if(quat_map.w() >= 0.) // comes from the log3 for quaternions which may change the
509  // sign.
510  if (quat_map.coeffs()[3] >= 0.) // comes from the log3 for quaternions which may change the
511  // sign.
512  PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t, J).noalias() = Jexp3QuatCoeffWise * Jlog;
513  else
514  PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t, J).noalias() = -Jexp3QuatCoeffWise * Jlog;
515 
516  // Jexp3(quat_map,PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t,J).template
517  // topLeftCorner<NQ,NV>());
518  }
519 
520  template<class Config_t, class Tangent_t, class JacobianOut_t>
521  static void dIntegrate_dq_impl(
522  const Eigen::MatrixBase<Config_t> & /*q*/,
523  const Eigen::MatrixBase<Tangent_t> & v,
524  const Eigen::MatrixBase<JacobianOut_t> & J,
525  const AssignmentOperatorType op = SETTO)
526  {
527  JacobianOut_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, J);
528  switch (op)
529  {
530  case SETTO:
531  Jout = exp3(-v);
532  break;
533  case ADDTO:
534  Jout += exp3(-v);
535  break;
536  case RMTO:
537  Jout -= exp3(-v);
538  break;
539  default:
540  assert(false && "Wrong Op requesed value");
541  break;
542  }
543  }
544 
545  template<class Config_t, class Tangent_t, class JacobianOut_t>
546  static void dIntegrate_dv_impl(
547  const Eigen::MatrixBase<Config_t> & /*q*/,
548  const Eigen::MatrixBase<Tangent_t> & v,
549  const Eigen::MatrixBase<JacobianOut_t> & J,
550  const AssignmentOperatorType op = SETTO)
551  {
552  switch (op)
553  {
554  case SETTO:
555  Jexp3<SETTO>(v, J.derived());
556  break;
557  case ADDTO:
558  Jexp3<ADDTO>(v, J.derived());
559  break;
560  case RMTO:
561  Jexp3<RMTO>(v, J.derived());
562  break;
563  default:
564  assert(false && "Wrong Op requesed value");
565  break;
566  }
567  }
568 
569  template<class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
570  static void dIntegrateTransport_dq_impl(
571  const Eigen::MatrixBase<Config_t> & /*q*/,
572  const Eigen::MatrixBase<Tangent_t> & v,
573  const Eigen::MatrixBase<JacobianIn_t> & Jin,
574  const Eigen::MatrixBase<JacobianOut_t> & J_out)
575  {
576  typedef typename SE3::Matrix3 Matrix3;
577  JacobianOut_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, J_out);
578  const Matrix3 Jtmp3 = exp3(-v);
579  Jout.noalias() = Jtmp3 * Jin;
580  }
581 
582  template<class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
583  static void dIntegrateTransport_dv_impl(
584  const Eigen::MatrixBase<Config_t> & /*q*/,
585  const Eigen::MatrixBase<Tangent_t> & v,
586  const Eigen::MatrixBase<JacobianIn_t> & Jin,
587  const Eigen::MatrixBase<JacobianOut_t> & J_out)
588  {
589  typedef typename SE3::Matrix3 Matrix3;
590  JacobianOut_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, J_out);
591  PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
592  PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED
593  Matrix3 Jtmp3;
594  Jexp3<SETTO>(v, Jtmp3);
595  PINOCCHIO_COMPILER_DIAGNOSTIC_POP
596  Jout.noalias() = Jtmp3 * Jin;
597  }
598 
599  template<class Config_t, class Tangent_t, class Jacobian_t>
600  static void dIntegrateTransport_dq_impl(
601  const Eigen::MatrixBase<Config_t> & /*q*/,
602  const Eigen::MatrixBase<Tangent_t> & v,
603  const Eigen::MatrixBase<Jacobian_t> & J_out)
604  {
605  typedef typename SE3::Matrix3 Matrix3;
606  Jacobian_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t, J_out);
607  const Matrix3 Jtmp3 = exp3(-v);
608  Jout = Jtmp3 * Jout;
609  }
610 
611  template<class Config_t, class Tangent_t, class Jacobian_t>
612  static void dIntegrateTransport_dv_impl(
613  const Eigen::MatrixBase<Config_t> & /*q*/,
614  const Eigen::MatrixBase<Tangent_t> & v,
615  const Eigen::MatrixBase<Jacobian_t> & J_out)
616  {
617  typedef typename SE3::Matrix3 Matrix3;
618  Jacobian_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t, J_out);
619  PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
620  PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED
621  Matrix3 Jtmp3;
622  Jexp3<SETTO>(v, Jtmp3);
623  PINOCCHIO_COMPILER_DIAGNOSTIC_POP
624  Jout = Jtmp3 * Jout;
625  }
626 
627  template<class ConfigL_t, class ConfigR_t, class ConfigOut_t>
628  static void interpolate_impl(
629  const Eigen::MatrixBase<ConfigL_t> & q0,
630  const Eigen::MatrixBase<ConfigR_t> & q1,
631  const Scalar & u,
632  const Eigen::MatrixBase<ConfigOut_t> & qout)
633  {
634  ConstQuaternionMap_t quat0(q0.derived().data());
635  assert(quaternion::isNormalized(quat0));
636  ConstQuaternionMap_t quat1(q1.derived().data());
637  assert(quaternion::isNormalized(quat1));
638 
639  QuaternionMap_t quat_res(PINOCCHIO_EIGEN_CONST_CAST(ConfigOut_t, qout).data());
640 
641  TangentVector_t w;
642  difference_impl(q0, q1, w);
643  integrate_impl(q0, u * w, qout);
644  assert(quaternion::isNormalized(quat_res));
645  }
646 
647  template<class ConfigL_t, class ConfigR_t>
648  static Scalar squaredDistance_impl(
649  const Eigen::MatrixBase<ConfigL_t> & q0, const Eigen::MatrixBase<ConfigR_t> & q1)
650  {
651  PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
652  PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED
653  TangentVector_t t;
654  difference_impl(q0, q1, t);
655  PINOCCHIO_COMPILER_DIAGNOSTIC_POP
656  return t.squaredNorm();
657  }
658 
659  template<class Config_t>
660  static void normalize_impl(const Eigen::MatrixBase<Config_t> & qout)
661  {
662  pinocchio::normalize(qout.const_cast_derived());
663  }
664 
665  template<class Config_t>
666  static bool isNormalized_impl(const Eigen::MatrixBase<Config_t> & qin, const Scalar & prec)
667  {
668  const Scalar norm = qin.norm();
669  using std::abs;
670  return abs(norm - Scalar(1.0)) < prec;
671  }
672 
673  template<class Config_t>
674  static void random_impl(const Eigen::MatrixBase<Config_t> & qout)
675  {
676  QuaternionMap_t quat_map(PINOCCHIO_EIGEN_CONST_CAST(Config_t, qout).data());
677  quaternion::uniformRandom(quat_map);
678 
679  assert(quaternion::isNormalized(quat_map));
680  }
681 
682  template<class ConfigL_t, class ConfigR_t, class ConfigOut_t>
683  static void randomConfiguration_impl(
684  const Eigen::MatrixBase<ConfigL_t> &,
685  const Eigen::MatrixBase<ConfigR_t> &,
686  const Eigen::MatrixBase<ConfigOut_t> & qout)
687  {
688  random_impl(qout);
689  }
690 
691  template<class ConfigL_t, class ConfigR_t>
692  static bool isSameConfiguration_impl(
693  const Eigen::MatrixBase<ConfigL_t> & q0,
694  const Eigen::MatrixBase<ConfigR_t> & q1,
695  const Scalar & prec)
696  {
697  ConstQuaternionMap_t quat1(q0.derived().data());
698  assert(quaternion::isNormalized(quat1));
699  ConstQuaternionMap_t quat2(q1.derived().data());
700  assert(quaternion::isNormalized(quat1));
701 
702  return quaternion::defineSameRotation(quat1, quat2, prec);
703  }
704  }; // struct SpecialOrthogonalOperationTpl<3,_Scalar,_Options>
705 
706 } // namespace pinocchio
707 
708 #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:90
pinocchio::quaternion::isNormalized
bool isNormalized(const Eigen::QuaternionBase< Quaternion > &quat, const typename Quaternion::Coefficients::RealScalar &prec)
Check whether the input quaternion is Normalized within the given precision.
Definition: quaternion.hpp:230
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:58
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:115
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