special-orthogonal.hpp
Go to the documentation of this file.
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>
19  {};
20 
21  template<int Dim, typename Scalar, int 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  {
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 
68  Scalar theta =
70  Scalar(0), // then
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 
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  assert (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  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) const
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  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) const
253  {
254  PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,Jout) = Jin;
255  }
256 
257  template <class Config_t, class Tangent_t, class Jacobian_t>
258  void dIntegrateTransport_dq_impl(const Eigen::MatrixBase<Config_t > & /*q*/,
259  const Eigen::MatrixBase<Tangent_t> & /*v*/,
260  const Eigen::MatrixBase<Jacobian_t> & /*J*/) const {}
261 
262  template <class Config_t, class Tangent_t, class Jacobian_t>
263  void dIntegrateTransport_dv_impl(const Eigen::MatrixBase<Config_t > & /*q*/,
264  const Eigen::MatrixBase<Tangent_t> & /*v*/,
265  const Eigen::MatrixBase<Jacobian_t> & /*J*/) const {}
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  void random_impl (const Eigen::MatrixBase<Config_t>& qout) const
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  void randomConfiguration_impl(const Eigen::MatrixBase<ConfigL_t> &,
330  const Eigen::MatrixBase<ConfigR_t> &,
331  const Eigen::MatrixBase<ConfigOut_t> & qout) const
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  {
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;
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 
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());
382  ConstQuaternionMap_t quat1 (q1.derived().data());
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  void dDifference_impl (const Eigen::MatrixBase<ConfigL_t> & q0,
391  const Eigen::MatrixBase<ConfigR_t> & q1,
392  const Eigen::MatrixBase<JacobianOut_t> & J) const
393  {
394  typedef typename SE3::Matrix3 Matrix3;
395 
396  ConstQuaternionMap_t quat0 (q0.derived().data());
398  ConstQuaternionMap_t quat1 (q1.derived().data());
400 
401  const Quaternion_t quat_diff = quat0.conjugate() * quat1;
402 
403  if (arg == ARG0) {
404  JacobianMatrix_t J1;
405  quaternion::Jlog3(quat_diff, J1);
406  const Matrix3 R = (quat_diff).matrix();
407 
408  PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J).noalias() = - J1 * R.transpose();
409  } else if (arg == ARG1) {
410  quaternion::Jlog3(quat_diff, J);
411  }
412  }
413 
414  template <class ConfigIn_t, class Velocity_t, class ConfigOut_t>
415  static void integrate_impl(const Eigen::MatrixBase<ConfigIn_t> & q,
416  const Eigen::MatrixBase<Velocity_t> & v,
417  const Eigen::MatrixBase<ConfigOut_t> & qout)
418  {
419  ConstQuaternionMap_t quat(q.derived().data());
421  QuaternionMap_t quat_map(PINOCCHIO_EIGEN_CONST_CAST(ConfigOut_t,qout).data());
422 
423  Quaternion_t pOmega; quaternion::exp3(v,pOmega);
424  quat_map = quat * pOmega;
427  }
428 
429  template <class Config_t, class Jacobian_t>
430  static void integrateCoeffWiseJacobian_impl(const Eigen::MatrixBase<Config_t> & q,
431  const Eigen::MatrixBase<Jacobian_t> & J)
432  {
433  assert(J.rows() == nq() && J.cols() == nv() && "J is not of the right dimension");
434 
435  typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(Jacobian_t) JacobianPlainType;
436  typedef typename SE3::Vector3 Vector3;
437  typedef typename SE3::Matrix3 Matrix3;
438 
439  ConstQuaternionMap_t quat_map(q.derived().data());
441 
442  Eigen::Matrix<Scalar,NQ,NV,JacobianPlainType::Options|Eigen::RowMajor> Jexp3QuatCoeffWise;
443 
444  Scalar theta;
445  const Vector3 v = quaternion::log3(quat_map,theta);
446  quaternion::Jexp3CoeffWise(v,Jexp3QuatCoeffWise);
447  Matrix3 Jlog;
448  Jlog3(theta,v,Jlog);
449 
450 // if(quat_map.w() >= 0.) // comes from the log3 for quaternions which may change the sign.
451  if(quat_map.coeffs()[3] >= 0.) // comes from the log3 for quaternions which may change the sign.
452  PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t,J).noalias() = Jexp3QuatCoeffWise * Jlog;
453  else
454  PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t,J).noalias() = -Jexp3QuatCoeffWise * Jlog;
455 
456 // Jexp3(quat_map,PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t,J).template topLeftCorner<NQ,NV>());
457  }
458 
459  template <class Config_t, class Tangent_t, class JacobianOut_t>
460  static void dIntegrate_dq_impl(const Eigen::MatrixBase<Config_t > & /*q*/,
461  const Eigen::MatrixBase<Tangent_t> & v,
462  const Eigen::MatrixBase<JacobianOut_t> & J,
463  const AssignmentOperatorType op=SETTO)
464  {
465  JacobianOut_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J);
466  switch(op)
467  {
468  case SETTO:
469  Jout = exp3(-v);
470  break;
471  case ADDTO:
472  Jout += exp3(-v);
473  break;
474  case RMTO:
475  Jout -= exp3(-v);
476  break;
477  default:
478  assert(false && "Wrong Op requesed value");
479  break;
480  }
481  }
482 
483  template <class Config_t, class Tangent_t, class JacobianOut_t>
484  static void dIntegrate_dv_impl(const Eigen::MatrixBase<Config_t > & /*q*/,
485  const Eigen::MatrixBase<Tangent_t> & v,
486  const Eigen::MatrixBase<JacobianOut_t> & J,
487  const AssignmentOperatorType op=SETTO)
488  {
489  switch(op)
490  {
491  case SETTO:
492  Jexp3<SETTO>(v, J.derived());
493  break;
494  case ADDTO:
495  Jexp3<ADDTO>(v, J.derived());
496  break;
497  case RMTO:
498  Jexp3<RMTO>(v, J.derived());
499  break;
500  default:
501  assert(false && "Wrong Op requesed value");
502  break;
503  }
504  }
505 
506  template <class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
507  void dIntegrateTransport_dq_impl(const Eigen::MatrixBase<Config_t > & /*q*/,
508  const Eigen::MatrixBase<Tangent_t> & v,
509  const Eigen::MatrixBase<JacobianIn_t> & Jin,
510  const Eigen::MatrixBase<JacobianOut_t> & J_out) const
511  {
512  typedef typename SE3::Matrix3 Matrix3;
513  JacobianOut_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J_out);
514  const Matrix3 Jtmp3 = exp3(-v);
515  Jout.noalias() = Jtmp3 * Jin;
516  }
517 
518  template <class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
519  void dIntegrateTransport_dv_impl(const Eigen::MatrixBase<Config_t > & /*q*/,
520  const Eigen::MatrixBase<Tangent_t> & v,
521  const Eigen::MatrixBase<JacobianIn_t> & Jin,
522  const Eigen::MatrixBase<JacobianOut_t> & J_out) const
523  {
524  typedef typename SE3::Matrix3 Matrix3;
525  JacobianOut_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J_out);
526  Matrix3 Jtmp3;
527  Jexp3<SETTO>(v, Jtmp3);
528  Jout.noalias() = Jtmp3 * Jin;
529  }
530 
531  template <class Config_t, class Tangent_t, class Jacobian_t>
532  void dIntegrateTransport_dq_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  const Matrix3 Jtmp3 = exp3(-v);
539  Jout = Jtmp3 * Jout;
540  }
541 
542  template <class Config_t, class Tangent_t, class Jacobian_t>
543  void dIntegrateTransport_dv_impl(const Eigen::MatrixBase<Config_t > & /*q*/,
544  const Eigen::MatrixBase<Tangent_t> & v,
545  const Eigen::MatrixBase<Jacobian_t> & J_out) const
546  {
547  typedef typename SE3::Matrix3 Matrix3;
548  Jacobian_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t,J_out);
549  Matrix3 Jtmp3;
550  Jexp3<SETTO>(v, Jtmp3);
551  Jout = Jtmp3 * Jout;
552  }
553 
554 
555  template <class ConfigL_t, class ConfigR_t, class ConfigOut_t>
556  static void interpolate_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
557  const Eigen::MatrixBase<ConfigR_t> & q1,
558  const Scalar & u,
559  const Eigen::MatrixBase<ConfigOut_t> & qout)
560  {
561  ConstQuaternionMap_t quat0 (q0.derived().data());
563  ConstQuaternionMap_t quat1 (q1.derived().data());
565 
566  QuaternionMap_t quat_map(PINOCCHIO_EIGEN_CONST_CAST(ConfigOut_t,qout).data());
567 
568  quat_map = quat0.slerp(u, quat1);
570  }
571 
572  template <class ConfigL_t, class ConfigR_t>
573  static Scalar squaredDistance_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
574  const Eigen::MatrixBase<ConfigR_t> & q1)
575  {
577  difference_impl(q0, q1, t);
578  return t.squaredNorm();
579  }
580 
581  template <class Config_t>
582  static void normalize_impl(const Eigen::MatrixBase<Config_t> & qout)
583  {
584  Config_t & qout_ = PINOCCHIO_EIGEN_CONST_CAST(Config_t,qout);
585  qout_.normalize();
586  }
587 
588  template <class Config_t>
589  static bool isNormalized_impl (const Eigen::MatrixBase<Config_t> & qin,
590  const Scalar& prec)
591  {
592  const Scalar norm = qin.norm();
593  using std::abs;
594  return abs(norm - Scalar(1.0)) < prec;
595  }
596 
597  template <class Config_t>
598  void random_impl(const Eigen::MatrixBase<Config_t> & qout) const
599  {
600  QuaternionMap_t quat_map(PINOCCHIO_EIGEN_CONST_CAST(Config_t,qout).data());
601  quaternion::uniformRandom(quat_map);
602 
604  }
605 
606  template <class ConfigL_t, class ConfigR_t, class ConfigOut_t>
607  void randomConfiguration_impl(const Eigen::MatrixBase<ConfigL_t> &,
608  const Eigen::MatrixBase<ConfigR_t> &,
609  const Eigen::MatrixBase<ConfigOut_t> & qout) const
610  {
611  random_impl(qout);
612  }
613 
614  template <class ConfigL_t, class ConfigR_t>
615  static bool isSameConfiguration_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
616  const Eigen::MatrixBase<ConfigR_t> & q1,
617  const Scalar & prec)
618  {
619  ConstQuaternionMap_t quat1(q0.derived().data());
621  ConstQuaternionMap_t quat2(q1.derived().data());
623 
624  return quaternion::defineSameRotation(quat1,quat2,prec);
625  }
626  }; // struct SpecialOrthogonalOperationTpl<3,_Scalar,_Options>
627 
628 } // namespace pinocchio
629 
630 #endif // ifndef __pinocchio_multibody_liegroup_special_orthogonal_operation_hpp__
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > const Eigen::MatrixBase< TangentVectorType > & v
static bool isSameConfiguration_impl(const Eigen::MatrixBase< ConfigL_t > &q0, const Eigen::MatrixBase< ConfigR_t > &q1, const Scalar &prec)
int nv(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointNvVisitor to get the dimension of the joint tangent space...
static void integrateCoeffWiseJacobian_impl(const Eigen::MatrixBase< Config_t > &q, const Eigen::MatrixBase< Jacobian_t > &J)
void dIntegrateTransport_dv_impl(const Eigen::MatrixBase< Config_t > &, const Eigen::MatrixBase< Tangent_t > &v, const Eigen::MatrixBase< JacobianIn_t > &Jin, const Eigen::MatrixBase< JacobianOut_t > &J_out) const
int NQ
Definition: dpendulum.py:8
Vec3f n
static bool isNormalized_impl(const Eigen::MatrixBase< Config_t > &qin, const Scalar &prec)
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.
static void difference_impl(const Eigen::MatrixBase< ConfigL_t > &q0, const Eigen::MatrixBase< ConfigR_t > &q1, const Eigen::MatrixBase< Tangent_t > &d)
int nq(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointNqVisitor to get the dimension of the joint configuration space...
void Jlog3(const Scalar &theta, const Eigen::MatrixBase< Vector3Like > &log, const Eigen::MatrixBase< Matrix3Like > &Jlog)
Derivative of log3.
void dIntegrateTransport_dq_impl(const Eigen::MatrixBase< Config_t > &, const Eigen::MatrixBase< Tangent_t > &, const Eigen::MatrixBase< Jacobian_t > &) const
void dIntegrateTransport_dq_impl(const Eigen::MatrixBase< Config_t > &, const Eigen::MatrixBase< Tangent_t > &v, const Eigen::MatrixBase< Jacobian_t > &J_out) const
void random_impl(const Eigen::MatrixBase< Config_t > &qout) const
static void normalize_impl(const Eigen::MatrixBase< Config_t > &qout)
void dDifference_impl(const Eigen::MatrixBase< ConfigL_t > &q0, const Eigen::MatrixBase< ConfigR_t > &q1, const Eigen::MatrixBase< JacobianOut_t > &J) const
static void interpolate_impl(const Eigen::MatrixBase< ConfigL_t > &q0, const Eigen::MatrixBase< ConfigR_t > &q1, const Scalar &u, const Eigen::MatrixBase< ConfigOut_t > &qout)
static Index nv()
Get dimension of Lie Group tangent space.
void firstOrderNormalize(const Eigen::QuaternionBase< D > &q)
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...
#define PINOCCHIO_LIE_GROUP_TPL_PUBLIC_INTERFACE(Derived)
void Jlog3(const Eigen::QuaternionBase< QuaternionLike > &quat, const Eigen::MatrixBase< Matrix3Like > &Jlog)
Computes the Jacobian of log3 operator for a unit quaternion.
static void difference_impl(const Eigen::MatrixBase< ConfigL_t > &q0, const Eigen::MatrixBase< ConfigR_t > &q1, const Eigen::MatrixBase< Tangent_t > &d)
void exp3(const Eigen::MatrixBase< Vector3Like > &v, Eigen::QuaternionBase< QuaternionLike > &quat_out)
Exp: so3 -> SO3 (quaternion)
void random_impl(const Eigen::MatrixBase< Config_t > &qout) const
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > & q
#define PINOCCHIO_EIGEN_CONST_CAST(TYPE, OBJ)
Macro for an automatic const_cast.
void Jexp3CoeffWise(const Eigen::MatrixBase< Vector3Like > &v, const Eigen::MatrixBase< Matrix43Like > &Jexp)
Derivative of where is a small perturbation of at identity.
SE3::Scalar Scalar
Definition: conversions.cpp:13
void dDifference_impl(const Eigen::MatrixBase< ConfigL_t > &q0, const Eigen::MatrixBase< ConfigR_t > &q1, const Eigen::MatrixBase< JacobianOut_t > &J) const
Eigen::Matrix< typename Vector3Like::Scalar, 3, 3, PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like)::Options > exp3(const Eigen::MatrixBase< Vector3Like > &v)
Exp: so3 -> SO3.
void dIntegrateTransport_dv_impl(const Eigen::MatrixBase< Config_t > &, const Eigen::MatrixBase< Tangent_t > &, const Eigen::MatrixBase< JacobianIn_t > &Jin, const Eigen::MatrixBase< JacobianOut_t > &Jout) const
static Index nv()
Get dimension of Lie Group tangent space.
static void integrate_impl(const Eigen::MatrixBase< ConfigIn_t > &q, const Eigen::MatrixBase< Velocity_t > &v, const Eigen::MatrixBase< ConfigOut_t > &qout)
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorIn1 > const Eigen::MatrixBase< ConfigVectorIn2 > const Scalar & u
AssignmentOperatorType
Definition: src/fwd.hpp:68
static void normalize_impl(const Eigen::MatrixBase< Config_t > &qout)
Eigen::Matrix< typename QuaternionLike::Scalar, 3, 1, PINOCCHIO_EIGEN_PLAIN_TYPE(typename QuaternionLike::Vector3)::Options > log3(const Eigen::QuaternionBase< QuaternionLike > &quat, typename QuaternionLike::Scalar &theta)
Same as log3 but with a unit quaternion as input.
static Matrix2Like::Scalar log(const Eigen::MatrixBase< Matrix2Like > &R)
static Matrix2Like::Scalar Jlog(const Eigen::MatrixBase< Matrix2Like > &)
static void dIntegrate_dq_impl(const Eigen::MatrixBase< Config_t > &, const Eigen::MatrixBase< Tangent_t > &, const Eigen::MatrixBase< JacobianOut_t > &J, const AssignmentOperatorType op=SETTO)
static Scalar squaredDistance_impl(const Eigen::MatrixBase< ConfigL_t > &q0, const Eigen::MatrixBase< ConfigR_t > &q1)
static void interpolate_impl(const Eigen::MatrixBase< ConfigL_t > &q0, const Eigen::MatrixBase< ConfigR_t > &q1, const Scalar &u, const Eigen::MatrixBase< ConfigOut_t > &qout)
void dIntegrateTransport_dv_impl(const Eigen::MatrixBase< Config_t > &, const Eigen::MatrixBase< Tangent_t > &, const Eigen::MatrixBase< Jacobian_t > &) const
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorIn1 > const Eigen::MatrixBase< ConfigVectorIn2 > & q1
R
quat
FCL_REAL d
void dIntegrateTransport_dv_impl(const Eigen::MatrixBase< Config_t > &, const Eigen::MatrixBase< Tangent_t > &v, const Eigen::MatrixBase< Jacobian_t > &J_out) const
void SINCOS(const S1 &a, S2 *sa, S3 *ca)
Computes sin/cos values of a given input scalar.
Definition: sincos.hpp:26
traits< SE3Tpl >::Vector3 Vector3
static void dIntegrate_dv_impl(const Eigen::MatrixBase< Config_t > &, const Eigen::MatrixBase< Tangent_t > &v, const Eigen::MatrixBase< JacobianOut_t > &J, const AssignmentOperatorType op=SETTO)
void uniformRandom(const Eigen::QuaternionBase< Derived > &q)
Uniformly random quaternion sphere.
Main pinocchio namespace.
Definition: timings.cpp:28
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorIn1 > & q0
PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorType) integrate(const ModelTpl< Scalar
Integrate a configuration vector for the specified model for a tangent vector during one unit time...
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.
#define PINOCCHIO_DEFAULT_QUATERNION_NORM_TOLERANCE_VALUE
Common traits structure to fully define base classes for CRTP.
Definition: src/fwd.hpp:44
if_then_else_impl< LhsType, RhsType, ThenType, ElseType >::ReturnType if_then_else(const ComparisonOperators op, const LhsType &lhs_value, const RhsType &rhs_value, const ThenType &then_value, const ElseType &else_value)
Transform3f t
static bool isNormalized_impl(const Eigen::MatrixBase< Config_t > &qin, const Scalar &prec)
void dIntegrateTransport_dq_impl(const Eigen::MatrixBase< Config_t > &, const Eigen::MatrixBase< Tangent_t > &v, const Eigen::MatrixBase< JacobianIn_t > &Jin, const Eigen::MatrixBase< JacobianOut_t > &J_out) const
void dIntegrateTransport_dq_impl(const Eigen::MatrixBase< Config_t > &, const Eigen::MatrixBase< Tangent_t > &, const Eigen::MatrixBase< JacobianIn_t > &Jin, const Eigen::MatrixBase< JacobianOut_t > &Jout) const
static void integrateCoeffWiseJacobian_impl(const Eigen::MatrixBase< Config_t > &q, const Eigen::MatrixBase< Jacobian_t > &J)
void randomConfiguration_impl(const Eigen::MatrixBase< ConfigL_t > &, const Eigen::MatrixBase< ConfigR_t > &, const Eigen::MatrixBase< ConfigOut_t > &qout) const
traits< SE3Tpl >::Matrix3 Matrix3
static void integrate_impl(const Eigen::MatrixBase< ConfigIn_t > &q, const Eigen::MatrixBase< Velocity_t > &v, const Eigen::MatrixBase< ConfigOut_t > &qout)
NV
Definition: dcrba.py:444
w
Definition: ur5x4.py:45
static void dIntegrate_dv_impl(const Eigen::MatrixBase< Config_t > &, const Eigen::MatrixBase< Tangent_t > &, const Eigen::MatrixBase< JacobianOut_t > &J, const AssignmentOperatorType op=SETTO)
void randomConfiguration_impl(const Eigen::MatrixBase< ConfigL_t > &, const Eigen::MatrixBase< ConfigR_t > &, const Eigen::MatrixBase< ConfigOut_t > &qout) const
static void dIntegrate_dq_impl(const Eigen::MatrixBase< Config_t > &, const Eigen::MatrixBase< Tangent_t > &v, const Eigen::MatrixBase< JacobianOut_t > &J, const AssignmentOperatorType op=SETTO)


pinocchio
Author(s):
autogenerated on Fri Jun 23 2023 02:38:32