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