pinocchio  2.4.4
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) - 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  void dDifference_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
147  const Eigen::MatrixBase<ConfigR_t> & q1,
148  const Eigen::MatrixBase<JacobianOut_t> & J) const
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  }
180 
181  template <class Config_t, class Jacobian_t>
182  static void integrateCoeffWiseJacobian_impl(const Eigen::MatrixBase<Config_t> & q,
183  const Eigen::MatrixBase<Jacobian_t> & J)
184  {
185  assert(J.rows() == nq() && J.cols() == nv() && "J is not of the right dimension");
186  Jacobian_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t,J);
187  Jout << -q[1], q[0];
188  }
189 
190  template <class Config_t, class Tangent_t, class JacobianOut_t>
191  static void dIntegrate_dq_impl(const Eigen::MatrixBase<Config_t > & /*q*/,
192  const Eigen::MatrixBase<Tangent_t> & /*v*/,
193  const Eigen::MatrixBase<JacobianOut_t> & J,
194  const AssignmentOperatorType op=SETTO)
195  {
196  JacobianOut_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J);
197  switch(op)
198  {
199  case SETTO:
200  Jout(0,0) = Scalar(1);
201  break;
202  case ADDTO:
203  Jout(0,0) += Scalar(1);
204  break;
205  case RMTO:
206  Jout(0,0) -= Scalar(1);
207  break;
208  default:
209  assert(false && "Wrong Op requesed value");
210  break;
211  }
212  }
213 
214  template <class Config_t, class Tangent_t, class JacobianOut_t>
215  static void dIntegrate_dv_impl(const Eigen::MatrixBase<Config_t > & /*q*/,
216  const Eigen::MatrixBase<Tangent_t> & /*v*/,
217  const Eigen::MatrixBase<JacobianOut_t> & J,
218  const AssignmentOperatorType op=SETTO)
219  {
220  JacobianOut_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J);
221  switch(op)
222  {
223  case SETTO:
224  Jout(0,0) = Scalar(1);
225  break;
226  case ADDTO:
227  Jout(0,0) += Scalar(1);
228  break;
229  case RMTO:
230  Jout(0,0) -= Scalar(1);
231  break;
232  default:
233  assert(false && "Wrong Op requesed value");
234  break;
235  }
236  }
237 
238  template <class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
239  void dIntegrateTransport_dq_impl(const Eigen::MatrixBase<Config_t > & /*q*/,
240  const Eigen::MatrixBase<Tangent_t> & /*v*/,
241  const Eigen::MatrixBase<JacobianIn_t> & Jin,
242  const Eigen::MatrixBase<JacobianOut_t> & Jout) const
243  {
244  PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,Jout) = Jin;
245  }
246 
247  template <class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
248  void dIntegrateTransport_dv_impl(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) const
252  {
253  PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,Jout) = Jin;
254  }
255 
256  template <class Config_t, class Tangent_t, class Jacobian_t>
257  void dIntegrateTransport_dq_impl(const Eigen::MatrixBase<Config_t > & /*q*/,
258  const Eigen::MatrixBase<Tangent_t> & /*v*/,
259  const Eigen::MatrixBase<Jacobian_t> & /*J*/) const {}
260 
261  template <class Config_t, class Tangent_t, class Jacobian_t>
262  void dIntegrateTransport_dv_impl(const Eigen::MatrixBase<Config_t > & /*q*/,
263  const Eigen::MatrixBase<Tangent_t> & /*v*/,
264  const Eigen::MatrixBase<Jacobian_t> & /*J*/) const {}
265 
266 
267 
268  template <class ConfigL_t, class ConfigR_t, class ConfigOut_t>
269  static void interpolate_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
270  const Eigen::MatrixBase<ConfigR_t> & q1,
271  const Scalar& u,
272  const Eigen::MatrixBase<ConfigOut_t>& qout)
273  {
274  ConfigOut_t & out = PINOCCHIO_EIGEN_CONST_CAST(ConfigOut_t,qout);
275 
276  assert ( std::abs(q0.norm() - 1) < 1e-8 && "initial configuration not normalized");
277  assert ( std::abs(q1.norm() - 1) < 1e-8 && "final configuration not normalized");
278  Scalar cosTheta = q0.dot(q1);
279  Scalar sinTheta = q0(0)*q1(1) - q0(1)*q1(0);
280  Scalar theta = atan2(sinTheta, cosTheta);
281  assert (fabs (sin (theta) - sinTheta) < 1e-8);
282 
283  const Scalar PI_value = PI<Scalar>();
284 
285  if (fabs (theta) > 1e-6 && fabs (theta) < PI_value - 1e-6)
286  {
287  out = (sin ((1-u)*theta)/sinTheta) * q0
288  + (sin ( u *theta)/sinTheta) * q1;
289  }
290  else if (fabs (theta) < 1e-6) // theta = 0
291  {
292  out = (1-u) * q0 + u * q1;
293  }
294  else // theta = +-PI
295  {
296  Scalar theta0 = atan2 (q0(1), q0(0));
297  SINCOS(theta0,&out[1],&out[0]);
298  }
299  }
300 
301  template <class Config_t>
302  static void normalize_impl (const Eigen::MatrixBase<Config_t> & qout)
303  {
304  Config_t & qout_ = PINOCCHIO_EIGEN_CONST_CAST(Config_t,qout).derived();
305  qout_.normalize();
306  }
307 
308  template <class Config_t>
309  void random_impl (const Eigen::MatrixBase<Config_t>& qout) const
310  {
311  Config_t & out = PINOCCHIO_EIGEN_CONST_CAST(Config_t,qout);
312 
313  const Scalar PI_value = PI<Scalar>();
314  const Scalar angle = -PI_value + Scalar(2)* PI_value * ((Scalar)rand())/RAND_MAX;
315  SINCOS(angle, &out(1), &out(0));
316  }
317 
318  template <class ConfigL_t, class ConfigR_t, class ConfigOut_t>
319  void randomConfiguration_impl(const Eigen::MatrixBase<ConfigL_t> &,
320  const Eigen::MatrixBase<ConfigR_t> &,
321  const Eigen::MatrixBase<ConfigOut_t> & qout) const
322  {
323  random_impl(qout);
324  }
325  }; // struct SpecialOrthogonalOperationTpl<2,_Scalar,_Options>
326 
327  template<typename _Scalar, int _Options>
328  struct SpecialOrthogonalOperationTpl<3,_Scalar,_Options>
329  : public LieGroupBase< SpecialOrthogonalOperationTpl<3,_Scalar,_Options> >
330  {
331  PINOCCHIO_LIE_GROUP_TPL_PUBLIC_INTERFACE(SpecialOrthogonalOperationTpl);
332 
333  typedef Eigen::Quaternion<Scalar> Quaternion_t;
334  typedef Eigen::Map< Quaternion_t> QuaternionMap_t;
335  typedef Eigen::Map<const Quaternion_t> ConstQuaternionMap_t;
336  typedef SE3Tpl<Scalar,Options> SE3;
337  typedef typename Eigen::NumTraits<Scalar>::Real RealScalar;
338 
343  static Index nq()
344  {
345  return NQ;
346  }
347 
349  static Index nv()
350  {
351  return NV;
352  }
353 
354  static ConfigVector_t neutral()
355  {
356  ConfigVector_t n; n.setZero (); n[3] = Scalar(1);
357  return n;
358  }
359 
360  static std::string name()
361  {
362  return std::string("SO(3)");
363  }
364 
365  template <class ConfigL_t, class ConfigR_t, class Tangent_t>
366  static void difference_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
367  const Eigen::MatrixBase<ConfigR_t> & q1,
368  const Eigen::MatrixBase<Tangent_t> & d)
369  {
370  ConstQuaternionMap_t quat0 (q0.derived().data());
371  assert(quaternion::isNormalized(quat0,RealScalar(PINOCCHIO_DEFAULT_QUATERNION_NORM_TOLERANCE_VALUE)));
372  ConstQuaternionMap_t quat1 (q1.derived().data());
373  assert(quaternion::isNormalized(quat1,RealScalar(PINOCCHIO_DEFAULT_QUATERNION_NORM_TOLERANCE_VALUE)));
374 
375  PINOCCHIO_EIGEN_CONST_CAST(Tangent_t,d)
376  = log3((quat0.matrix().transpose() * quat1.matrix()).eval());
377  }
378 
379  template <ArgumentPosition arg, class ConfigL_t, class ConfigR_t, class JacobianOut_t>
380  void dDifference_impl (const Eigen::MatrixBase<ConfigL_t> & q0,
381  const Eigen::MatrixBase<ConfigR_t> & q1,
382  const Eigen::MatrixBase<JacobianOut_t> & J) const
383  {
384  typedef typename SE3::Matrix3 Matrix3;
385 
386  ConstQuaternionMap_t quat0 (q0.derived().data());
387  assert(quaternion::isNormalized(quat0,RealScalar(PINOCCHIO_DEFAULT_QUATERNION_NORM_TOLERANCE_VALUE)));
388  ConstQuaternionMap_t quat1 (q1.derived().data());
389  assert(quaternion::isNormalized(quat1,RealScalar(PINOCCHIO_DEFAULT_QUATERNION_NORM_TOLERANCE_VALUE)));
390 
391  const Matrix3 R = quat0.matrix().transpose() * quat1.matrix(); // TODO: perform first the Quaternion multiplications and then return a Rotation Matrix
392 
393  if (arg == ARG0) {
394  JacobianMatrix_t J1;
395  Jlog3 (R, J1);
396 
397  PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J).noalias() = - J1 * R.transpose();
398  } else if (arg == ARG1) {
399  Jlog3 (R, J);
400  }
401  }
402 
403  template <class ConfigIn_t, class Velocity_t, class ConfigOut_t>
404  static void integrate_impl(const Eigen::MatrixBase<ConfigIn_t> & q,
405  const Eigen::MatrixBase<Velocity_t> & v,
406  const Eigen::MatrixBase<ConfigOut_t> & qout)
407  {
408  ConstQuaternionMap_t quat(q.derived().data());
409  assert(quaternion::isNormalized(quat,RealScalar(PINOCCHIO_DEFAULT_QUATERNION_NORM_TOLERANCE_VALUE)));
410  QuaternionMap_t quat_map(PINOCCHIO_EIGEN_CONST_CAST(ConfigOut_t,qout).data());
411 
412  Quaternion_t pOmega; quaternion::exp3(v,pOmega);
413  quat_map = quat * pOmega;
414  quaternion::firstOrderNormalize(quat_map);
415  assert(quaternion::isNormalized(quat_map,RealScalar(PINOCCHIO_DEFAULT_QUATERNION_NORM_TOLERANCE_VALUE)));
416  }
417 
418  template <class Config_t, class Jacobian_t>
419  static void integrateCoeffWiseJacobian_impl(const Eigen::MatrixBase<Config_t> & q,
420  const Eigen::MatrixBase<Jacobian_t> & J)
421  {
422  assert(J.rows() == nq() && J.cols() == nv() && "J is not of the right dimension");
423 
424  typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(Jacobian_t) JacobianPlainType;
425  typedef typename SE3::Vector3 Vector3;
426  typedef typename SE3::Matrix3 Matrix3;
427 
428  ConstQuaternionMap_t quat_map(q.derived().data());
429  assert(quaternion::isNormalized(quat_map,RealScalar(PINOCCHIO_DEFAULT_QUATERNION_NORM_TOLERANCE_VALUE)));
430 
431  Eigen::Matrix<Scalar,NQ,NV,JacobianPlainType::Options|Eigen::RowMajor> Jexp3QuatCoeffWise;
432 
433  Scalar theta;
434  const Vector3 v = quaternion::log3(quat_map,theta);
435  quaternion::Jexp3CoeffWise(v,Jexp3QuatCoeffWise);
436  Matrix3 Jlog;
437  Jlog3(theta,v,Jlog);
438 
439 // if(quat_map.w() >= 0.) // comes from the log3 for quaternions which may change the sign.
440  if(quat_map.coeffs()[3] >= 0.) // comes from the log3 for quaternions which may change the sign.
441  PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t,J).noalias() = Jexp3QuatCoeffWise * Jlog;
442  else
443  PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t,J).noalias() = -Jexp3QuatCoeffWise * Jlog;
444 
445 // Jexp3(quat_map,PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t,J).template topLeftCorner<NQ,NV>());
446  }
447 
448  template <class Config_t, class Tangent_t, class JacobianOut_t>
449  static void dIntegrate_dq_impl(const Eigen::MatrixBase<Config_t > & /*q*/,
450  const Eigen::MatrixBase<Tangent_t> & v,
451  const Eigen::MatrixBase<JacobianOut_t> & J,
452  const AssignmentOperatorType op=SETTO)
453  {
454  JacobianOut_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J);
455  switch(op)
456  {
457  case SETTO:
458  Jout = exp3(-v);
459  break;
460  case ADDTO:
461  Jout += exp3(-v);
462  break;
463  case RMTO:
464  Jout -= exp3(-v);
465  break;
466  default:
467  assert(false && "Wrong Op requesed value");
468  break;
469  }
470  }
471 
472  template <class Config_t, class Tangent_t, class JacobianOut_t>
473  static void dIntegrate_dv_impl(const Eigen::MatrixBase<Config_t > & /*q*/,
474  const Eigen::MatrixBase<Tangent_t> & v,
475  const Eigen::MatrixBase<JacobianOut_t> & J,
476  const AssignmentOperatorType op=SETTO)
477  {
478  switch(op)
479  {
480  case SETTO:
481  Jexp3<SETTO>(v, J.derived());
482  break;
483  case ADDTO:
484  Jexp3<ADDTO>(v, J.derived());
485  break;
486  case RMTO:
487  Jexp3<RMTO>(v, J.derived());
488  break;
489  default:
490  assert(false && "Wrong Op requesed value");
491  break;
492  }
493  }
494 
495  template <class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
496  void dIntegrateTransport_dq_impl(const Eigen::MatrixBase<Config_t > & /*q*/,
497  const Eigen::MatrixBase<Tangent_t> & v,
498  const Eigen::MatrixBase<JacobianIn_t> & Jin,
499  const Eigen::MatrixBase<JacobianOut_t> & J_out) const
500  {
501  typedef typename SE3::Matrix3 Matrix3;
502  JacobianOut_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J_out);
503  const Matrix3 Jtmp3 = exp3(-v);
504  Jout.noalias() = Jtmp3 * Jin;
505  }
506 
507  template <class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
508  void dIntegrateTransport_dv_impl(const Eigen::MatrixBase<Config_t > & /*q*/,
509  const Eigen::MatrixBase<Tangent_t> & v,
510  const Eigen::MatrixBase<JacobianIn_t> & Jin,
511  const Eigen::MatrixBase<JacobianOut_t> & J_out) const
512  {
513  typedef typename SE3::Matrix3 Matrix3;
514  JacobianOut_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J_out);
515  Matrix3 Jtmp3;
516  Jexp3<SETTO>(v, Jtmp3);
517  Jout.noalias() = Jtmp3 * Jin;
518  }
519 
520  template <class Config_t, class Tangent_t, class Jacobian_t>
521  void dIntegrateTransport_dq_impl(const Eigen::MatrixBase<Config_t > & /*q*/,
522  const Eigen::MatrixBase<Tangent_t> & v,
523  const Eigen::MatrixBase<Jacobian_t> & J_out) const
524  {
525  typedef typename SE3::Matrix3 Matrix3;
526  Jacobian_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t,J_out);
527  const Matrix3 Jtmp3 = exp3(-v);
528  Jout = Jtmp3 * Jout;
529  }
530 
531  template <class Config_t, class Tangent_t, class Jacobian_t>
532  void dIntegrateTransport_dv_impl(const Eigen::MatrixBase<Config_t > & /*q*/,
533  const Eigen::MatrixBase<Tangent_t> & v,
534  const Eigen::MatrixBase<Jacobian_t> & J_out) const
535  {
536  typedef typename SE3::Matrix3 Matrix3;
537  Jacobian_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t,J_out);
538  Matrix3 Jtmp3;
539  Jexp3<SETTO>(v, Jtmp3);
540  Jout = Jtmp3 * Jout;
541  }
542 
543 
544  template <class ConfigL_t, class ConfigR_t, class ConfigOut_t>
545  static void interpolate_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
546  const Eigen::MatrixBase<ConfigR_t> & q1,
547  const Scalar & u,
548  const Eigen::MatrixBase<ConfigOut_t> & qout)
549  {
550  ConstQuaternionMap_t quat0 (q0.derived().data());
551  assert(quaternion::isNormalized(quat0,RealScalar(PINOCCHIO_DEFAULT_QUATERNION_NORM_TOLERANCE_VALUE)));
552  ConstQuaternionMap_t quat1 (q1.derived().data());
553  assert(quaternion::isNormalized(quat1,RealScalar(PINOCCHIO_DEFAULT_QUATERNION_NORM_TOLERANCE_VALUE)));
554 
555  QuaternionMap_t quat_map(PINOCCHIO_EIGEN_CONST_CAST(ConfigOut_t,qout).data());
556 
557  quat_map = quat0.slerp(u, quat1);
558  assert(quaternion::isNormalized(quat_map,RealScalar(PINOCCHIO_DEFAULT_QUATERNION_NORM_TOLERANCE_VALUE)));
559  }
560 
561  template <class ConfigL_t, class ConfigR_t>
562  static Scalar squaredDistance_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
563  const Eigen::MatrixBase<ConfigR_t> & q1)
564  {
565  TangentVector_t t;
566  difference_impl(q0, q1, t);
567  return t.squaredNorm();
568  }
569 
570  template <class Config_t>
571  static void normalize_impl(const Eigen::MatrixBase<Config_t> & qout)
572  {
573  Config_t & qout_ = PINOCCHIO_EIGEN_CONST_CAST(Config_t,qout);
574  qout_.normalize();
575  }
576 
577  template <class Config_t>
578  void random_impl(const Eigen::MatrixBase<Config_t> & qout) const
579  {
580  QuaternionMap_t quat_map(PINOCCHIO_EIGEN_CONST_CAST(Config_t,qout).data());
581  quaternion::uniformRandom(quat_map);
582 
583  assert(quaternion::isNormalized(quat_map,RealScalar(PINOCCHIO_DEFAULT_QUATERNION_NORM_TOLERANCE_VALUE)));
584  }
585 
586  template <class ConfigL_t, class ConfigR_t, class ConfigOut_t>
587  void randomConfiguration_impl(const Eigen::MatrixBase<ConfigL_t> &,
588  const Eigen::MatrixBase<ConfigR_t> &,
589  const Eigen::MatrixBase<ConfigOut_t> & qout) const
590  {
591  random_impl(qout);
592  }
593 
594  template <class ConfigL_t, class ConfigR_t>
595  static bool isSameConfiguration_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
596  const Eigen::MatrixBase<ConfigR_t> & q1,
597  const Scalar & prec)
598  {
599  ConstQuaternionMap_t quat1(q0.derived().data());
600  assert(quaternion::isNormalized(quat1,RealScalar(PINOCCHIO_DEFAULT_QUATERNION_NORM_TOLERANCE_VALUE)));
601  ConstQuaternionMap_t quat2(q1.derived().data());
602  assert(quaternion::isNormalized(quat1,RealScalar(PINOCCHIO_DEFAULT_QUATERNION_NORM_TOLERANCE_VALUE)));
603 
604  return quaternion::defineSameRotation(quat1,quat2,prec);
605  }
606  }; // struct SpecialOrthogonalOperationTpl<3,_Scalar,_Options>
607 
608 } // namespace pinocchio
609 
610 #endif // ifndef __pinocchio_multibody_liegroup_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:75
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::nq
int nq(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointNqVisitor to get the dimension of the joint configuration space...
pinocchio::Jlog3
void Jlog3(const Scalar &theta, const Eigen::MatrixBase< Vector3Like > &log, const Eigen::MatrixBase< Matrix3Like > &Jlog)
Derivative of log3.
Definition: explog.hpp:193
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::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::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:88
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::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:146
pinocchio::SpecialOrthogonalOperationTpl< 3, _Scalar, _Options >::nv
static Index nv()
Get dimension of Lie Group tangent space.
Definition: special-orthogonal.hpp:349
pinocchio::SpecialOrthogonalOperationTpl< 2, _Scalar, _Options >::nv
static Index nv()
Get dimension of Lie Group tangent space.
Definition: special-orthogonal.hpp:117
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::quaternion::uniformRandom
void uniformRandom(const Eigen::QuaternionBase< Derived > &q)
Uniformly random quaternion sphere.
Definition: quaternion.hpp:108
pinocchio
Main pinocchio namespace.
Definition: treeview.dox:24
pinocchio::SpecialOrthogonalOperationTpl< 2, _Scalar, _Options >::nq
static Index nq()
Definition: special-orthogonal.hpp:111
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::SpecialOrthogonalOperationTpl< 3, _Scalar, _Options >::nq
static Index nq()
Definition: special-orthogonal.hpp:343
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::traits
Common traits structure to fully define base classes for CRTP.
Definition: fwd.hpp:35