pinocchio  2.4.0-dirty
A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
special-euclidean.hpp
1 //
2 // Copyright (c) 2016-2019 CNRS INRIA
3 //
4 
5 #ifndef __pinocchio_special_euclidean_operation_hpp__
6 #define __pinocchio_special_euclidean_operation_hpp__
7 
8 #include <limits>
9 
10 #include "pinocchio/macros.hpp"
11 #include "pinocchio/spatial/fwd.hpp"
12 #include "pinocchio/utils/static-if.hpp"
13 #include "pinocchio/spatial/se3.hpp"
14 #include "pinocchio/multibody/liegroup/liegroup-base.hpp"
15 
16 #include "pinocchio/multibody/liegroup/vector-space.hpp"
17 #include "pinocchio/multibody/liegroup/cartesian-product.hpp"
18 #include "pinocchio/multibody/liegroup/special-orthogonal.hpp"
19 
20 namespace pinocchio
21 {
22  template<int Dim, typename Scalar, int Options = 0>
24  {};
25 
26  template<int Dim, typename Scalar, int Options>
27  struct traits< SpecialEuclideanOperationTpl<Dim,Scalar,Options> >
28  {};
29 
30  template<typename _Scalar, int _Options>
31  struct traits< SpecialEuclideanOperationTpl<2,_Scalar,_Options> >
32  {
33  typedef _Scalar Scalar;
34  enum
35  {
36  Options = _Options,
37  NQ = 4,
38  NV = 3
39  };
40  };
41 
42  // SE(2)
43  template<typename _Scalar, int _Options>
44  struct SpecialEuclideanOperationTpl<2,_Scalar,_Options>
45  : public LieGroupBase <SpecialEuclideanOperationTpl<2,_Scalar,_Options> >
46  {
47  PINOCCHIO_LIE_GROUP_TPL_PUBLIC_INTERFACE(SpecialEuclideanOperationTpl);
48 
52 
53  typedef Eigen::Matrix<Scalar,2,2,Options> Matrix2;
54  typedef Eigen::Matrix<Scalar,2,1,Options> Vector2;
55 
56  template<typename TangentVector, typename Matrix2Like, typename Vector2Like>
57  static void exp(const Eigen::MatrixBase<TangentVector> & v,
58  const Eigen::MatrixBase<Matrix2Like> & R,
59  const Eigen::MatrixBase<Vector2Like> & t)
60  {
61  EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(TangentVector_t,TangentVector);
62  EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix2Like, 2, 2);
63  EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector2Like, 2);
64 
65  typedef typename Matrix2Like::Scalar Scalar;
66  const Scalar omega = v(2);
67  Scalar cv,sv; SINCOS(omega, &sv, &cv);
68  PINOCCHIO_EIGEN_CONST_CAST(Matrix2Like,R) << cv, -sv, sv, cv;
69  using internal::if_then_else;
70 
71  {
72  typename PINOCCHIO_EIGEN_PLAIN_TYPE(Vector2Like) vcross(-v(1), v(0));
73  vcross -= -v(1)*R.col(0) + v(0)*R.col(1);
74  vcross /= omega;
75  Scalar omega_abs = math::fabs(omega);
76  PINOCCHIO_EIGEN_CONST_CAST(Vector2Like,t).coeffRef(0) = if_then_else(omega_abs > 1e-14,
77  vcross.coeff(0),
78  v.coeff(0));
79 
80  PINOCCHIO_EIGEN_CONST_CAST(Vector2Like,t).coeffRef(1) = if_then_else(omega_abs > 1e-14,
81  vcross.coeff(1),
82  v.coeff(1));
83  }
84 
85  }
86 
87  template<typename Matrix2Like, typename Vector2Like, typename Matrix3Like>
88  static void toInverseActionMatrix(const Eigen::MatrixBase<Matrix2Like> & R,
89  const Eigen::MatrixBase<Vector2Like> & t,
90  const Eigen::MatrixBase<Matrix3Like> & M)
91  {
92  EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix2Like, 2, 2);
93  EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector2Like, 2);
94  EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix3Like, 3, 3);
95 
96  typedef typename Matrix3Like::Scalar Scalar;
97 
98  Matrix3Like & Mout = PINOCCHIO_EIGEN_CONST_CAST(Matrix3Like,M);
99  Mout.template topLeftCorner<2,2>() = R.transpose();
100  typename PINOCCHIO_EIGEN_PLAIN_TYPE(Vector2Like) tinv(R.transpose() * t);
101  Mout.template topRightCorner<2,1>() << - tinv(1), tinv(0);
102  Mout.template bottomLeftCorner<1,2>().setZero();
103  Mout(2,2) = (Scalar)1;
104  }
105 
106  template<typename Matrix2Like, typename Vector2Like, typename TangentVector>
107  static void log(const Eigen::MatrixBase<Matrix2Like> & R,
108  const Eigen::MatrixBase<Vector2Like> & p,
109  const Eigen::MatrixBase<TangentVector> & v)
110  {
111  EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix2Like, 2, 2);
112  EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector2Like, 2);
113  EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(TangentVector_t,TangentVector);
114 
115  TangentVector & vout = PINOCCHIO_EIGEN_CONST_CAST(TangentVector,v);
116 
117  typedef typename Matrix2Like::Scalar Scalar1;
118 
119  Scalar1 t = SO2_t::log(R);
120  const Scalar1 tabs = math::fabs(t);
121  const Scalar1 t2 = t*t;
122  Scalar1 alpha;
123  if (tabs < 1e-4)
124  {
125  alpha = 1 - t2/12 - t2*t2/720;
126  }
127  else
128  {
129  Scalar1 st,ct; SINCOS(tabs, &st, &ct);
130  alpha = tabs*st/(2*(1-ct));
131  }
132 
133  vout.template head<2>().noalias() = alpha * p;
134  vout(0) += t/2 * p(1);
135  vout(1) += -t/2 * p(0);
136  vout(2) = t;
137  }
138 
139  template<typename Matrix2Like, typename Vector2Like, typename JacobianOutLike>
140  static void Jlog(const Eigen::MatrixBase<Matrix2Like> & R,
141  const Eigen::MatrixBase<Vector2Like> & p,
142  const Eigen::MatrixBase<JacobianOutLike> & J)
143  {
144  EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix2Like, 2, 2);
145  EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector2Like, 2);
146  EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(JacobianOutLike, JacobianMatrix_t);
147 
148  JacobianOutLike & Jout = PINOCCHIO_EIGEN_CONST_CAST(JacobianOutLike,J);
149 
150  typedef typename Matrix2Like::Scalar Scalar1;
151 
152  Scalar1 t = SO2_t::log(R);
153  const Scalar1 tabs = math::fabs(t);
154  Scalar1 alpha, alpha_dot;
155  if (tabs < 1e-4)
156  {
157  Scalar1 t2 = t*t;
158  alpha = 1 - t2/12;
159  alpha_dot = - t / 6 - t2*t / 180;
160  }
161  else
162  {
163  Scalar1 st,ct; SINCOS(t, &st, &ct);
164  Scalar1 inv_2_1_ct = 0.5 / (1-ct);
165  // t * sin(t) / (2 * (1 - cos(t)) )
166  alpha = t*st*inv_2_1_ct;
167  // [ ( 1 - cos(t) ) * sin(t) + t * cos(t) - 1 ] / (2 * (1 - cos(t))^2 )
168  alpha_dot = (st-t) * inv_2_1_ct;
169  }
170 
171  typename PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix2Like) V;
172  V(0,0) = V(1,1) = alpha;
173  V(1,0) = - t / 2;
174  V(0,1) = - V(1,0);
175 
176  Jout.template topLeftCorner <2,2>().noalias() = V * R;
177  Jout.template topRightCorner<2,1>() << alpha_dot*p[0] + p[1]/2, -p(0)/2 + alpha_dot*p(1);
178  Jout.template bottomLeftCorner<1,2>().setZero();
179  Jout(2,2) = 1;
180  }
181 
186  static Index nq()
187  {
188  return NQ;
189  }
191  static Index nv()
192  {
193  return NV;
194  }
195 
196  static ConfigVector_t neutral()
197  {
198  ConfigVector_t n; n << Scalar(0), Scalar(0), Scalar(1), Scalar(0);
199  return n;
200  }
201 
202  static std::string name()
203  {
204  return std::string("SE(2)");
205  }
206 
207  template <class ConfigL_t, class ConfigR_t, class Tangent_t>
208  static void difference_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
209  const Eigen::MatrixBase<ConfigR_t> & q1,
210  const Eigen::MatrixBase<Tangent_t> & d)
211  {
212  if (q0 == q1)
213  {
214  PINOCCHIO_EIGEN_CONST_CAST(Tangent_t,d).setZero();
215  return;
216  }
217  Matrix2 R0, R1; Vector2 t0, t1;
218  forwardKinematics(R0, t0, q0);
219  forwardKinematics(R1, t1, q1);
220  Matrix2 R (R0.transpose() * R1);
221  Vector2 t (R0.transpose() * (t1 - t0));
222 
223  log(R, t, d);
224  }
225 
226  template <ArgumentPosition arg, class ConfigL_t, class ConfigR_t, class JacobianOut_t>
227  void dDifference_impl (const Eigen::MatrixBase<ConfigL_t> & q0,
228  const Eigen::MatrixBase<ConfigR_t> & q1,
229  const Eigen::MatrixBase<JacobianOut_t>& J) const
230  {
231  Matrix2 R0, R1; Vector2 t0, t1;
232  forwardKinematics(R0, t0, q0);
233  forwardKinematics(R1, t1, q1);
234  Matrix2 R (R0.transpose() * R1);
235  Vector2 t (R0.transpose() * (t1 - t0));
236 
237  if (arg == ARG0) {
238  JacobianMatrix_t J1;
239  Jlog (R, t, J1);
240 
241  // pcross = [ y1-y0, - (x1 - x0) ]
242  Vector2 pcross (q1(1) - q0(1), q0(0) - q1(0));
243 
244  JacobianOut_t& J0 = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, J);
245  J0.template topLeftCorner <2,2> ().noalias() = - R.transpose();
246  J0.template topRightCorner<2,1> ().noalias() = R1.transpose() * pcross;
247  J0.template bottomLeftCorner <1,2> ().setZero();
248  J0 (2,2) = -1;
249  J0.applyOnTheLeft(J1);
250  } else if (arg == ARG1) {
251  Jlog (R, t, J);
252  }
253  }
254 
255  template <class ConfigIn_t, class Velocity_t, class ConfigOut_t>
256  static void integrate_impl(const Eigen::MatrixBase<ConfigIn_t> & q,
257  const Eigen::MatrixBase<Velocity_t> & v,
258  const Eigen::MatrixBase<ConfigOut_t> & qout)
259  {
260  ConfigOut_t & out = PINOCCHIO_EIGEN_CONST_CAST(ConfigOut_t,qout);
261 
262  Matrix2 R0, R;
263  Vector2 t0, t;
264  forwardKinematics(R0, t0, q);
265  exp(v, R, t);
266 
267  out.template head<2>().noalias() = R0 * t + t0;
268  out.template tail<2>().noalias() = R0 * R.col(0);
269  }
270 
271  template <class Config_t, class Jacobian_t>
272  static void integrateCoeffWiseJacobian_impl(const Eigen::MatrixBase<Config_t> & q,
273  const Eigen::MatrixBase<Jacobian_t> & J)
274  {
275  assert(J.rows() == nq() && J.cols() == nv() && "J is not of the right dimension");
276 
277  Jacobian_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t,J);
278  Jout.setZero();
279 
280  const typename Config_t::Scalar & c_theta = q(2),
281  & s_theta = q(3);
282 
283  Jout.template topLeftCorner<2,2>() << c_theta, -s_theta, s_theta, c_theta;
284  Jout.template bottomRightCorner<2,1>() << -s_theta, c_theta;
285  }
286 
287  template <class Config_t, class Tangent_t, class JacobianOut_t>
288  static void dIntegrate_dq_impl(const Eigen::MatrixBase<Config_t > & /*q*/,
289  const Eigen::MatrixBase<Tangent_t> & v,
290  const Eigen::MatrixBase<JacobianOut_t>& J)
291  {
292  JacobianOut_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J);
293 
294  Matrix2 R;
295  Vector2 t;
296  exp(v, R, t);
297 
298  toInverseActionMatrix(R, t, Jout);
299  }
300 
301  template <class Config_t, class Tangent_t, class JacobianOut_t>
302  static void dIntegrate_dv_impl(const Eigen::MatrixBase<Config_t > & /*q*/,
303  const Eigen::MatrixBase<Tangent_t> & v,
304  const Eigen::MatrixBase<JacobianOut_t> & J)
305  {
306  JacobianOut_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J);
307  // TODO sparse version
308  MotionTpl<Scalar,0> nu; nu.toVector() << v.template head<2>(), 0, 0, 0, v[2];
309  Eigen::Matrix<Scalar,6,6> Jtmp6;
310  Jexp6(nu, Jtmp6);
311  Jout << Jtmp6.template topLeftCorner<2,2>(), Jtmp6.template topRightCorner<2,1>(),
312  Jtmp6.template bottomLeftCorner<1,2>(), Jtmp6.template bottomRightCorner<1,1>();
313  }
314 
315  // interpolate_impl use default implementation.
316  // template <class ConfigL_t, class ConfigR_t, class ConfigOut_t>
317  // static void interpolate_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
318  // const Eigen::MatrixBase<ConfigR_t> & q1,
319  // const Scalar& u,
320  // const Eigen::MatrixBase<ConfigOut_t>& qout)
321 
322  // template <class ConfigL_t, class ConfigR_t>
323  // static double squaredDistance_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
324  // const Eigen::MatrixBase<ConfigR_t> & q1)
325  template <class Config_t>
326  static void normalize_impl (const Eigen::MatrixBase<Config_t>& qout)
327  {
328  PINOCCHIO_EIGEN_CONST_CAST(Config_t,qout).template tail<2>().normalize();
329  }
330 
331  template <class Config_t>
332  void random_impl (const Eigen::MatrixBase<Config_t>& qout) const
333  {
334  R2crossSO2_t().random(qout);
335  }
336 
337  template <class ConfigL_t, class ConfigR_t, class ConfigOut_t>
338  void randomConfiguration_impl(const Eigen::MatrixBase<ConfigL_t> & lower,
339  const Eigen::MatrixBase<ConfigR_t> & upper,
340  const Eigen::MatrixBase<ConfigOut_t> & qout)
341  const
342  {
343  R2crossSO2_t ().randomConfiguration(lower, upper, qout);
344  }
345 
346  template <class ConfigL_t, class ConfigR_t>
347  static bool isSameConfiguration_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
348  const Eigen::MatrixBase<ConfigR_t> & q1,
349  const Scalar & prec)
350  {
351  return R2crossSO2_t().isSameConfiguration(q0, q1, prec);
352  }
353 
354  protected:
355 
356  template<typename Matrix2Like, typename Vector2Like, typename Vector4Like>
357  static void forwardKinematics(const Eigen::MatrixBase<Matrix2Like> & R,
358  const Eigen::MatrixBase<Vector2Like> & t,
359  const Eigen::MatrixBase<Vector4Like> & q)
360  {
361  EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(Matrix2Like, Matrix2);
362  EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(Vector2Like, Vector2);
363  EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigVector_t,Vector4Like);
364 
365  PINOCCHIO_EIGEN_CONST_CAST(Vector2Like,t) = q.template head<2>();
366  const typename Vector4Like::Scalar & c_theta = q(2),
367  & s_theta = q(3);
368  PINOCCHIO_EIGEN_CONST_CAST(Matrix2Like,R) << c_theta, -s_theta, s_theta, c_theta;
369 
370  }
371  }; // struct SpecialEuclideanOperationTpl<2>
372 
373  template<typename _Scalar, int _Options>
374  struct traits< SpecialEuclideanOperationTpl<3,_Scalar,_Options> >
375  {
376  typedef _Scalar Scalar;
377  enum
378  {
379  Options = _Options,
380  NQ = 7,
381  NV = 6
382  };
383  };
384 
386  template<typename _Scalar, int _Options>
387  struct SpecialEuclideanOperationTpl<3,_Scalar,_Options>
388  : public LieGroupBase <SpecialEuclideanOperationTpl<3,_Scalar,_Options> >
389  {
390  PINOCCHIO_LIE_GROUP_TPL_PUBLIC_INTERFACE(SpecialEuclideanOperationTpl);
391 
393 
394  typedef Eigen::Quaternion<Scalar,Options> Quaternion_t;
395  typedef Eigen::Map< Quaternion_t> QuaternionMap_t;
396  typedef Eigen::Map<const Quaternion_t> ConstQuaternionMap_t;
398  typedef SE3Tpl<Scalar,Options> SE3;
399 
404  static Index nq()
405  {
406  return NQ;
407  }
409  static Index nv()
410  {
411  return NV;
412  }
413 
414  static ConfigVector_t neutral()
415  {
416  ConfigVector_t n; n.template head<6>().setZero(); n[6] = 1;
417  return n;
418  }
419 
420  static std::string name()
421  {
422  return std::string("SE(3)");
423  }
424 
425  template <class ConfigL_t, class ConfigR_t, class Tangent_t>
426  static void difference_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
427  const Eigen::MatrixBase<ConfigR_t> & q1,
428  const Eigen::MatrixBase<Tangent_t> & d)
429  {
430  if (q0 == q1)
431  {
432  PINOCCHIO_EIGEN_CONST_CAST(Tangent_t,d).setZero();
433  return;
434  }
435  ConstQuaternionMap_t p0 (q0.derived().template tail<4>().data());
436  ConstQuaternionMap_t p1 (q1.derived().template tail<4>().data());
437  PINOCCHIO_EIGEN_CONST_CAST(Tangent_t,d)
438  = log6( SE3(p0.matrix(), q0.derived().template head<3>()).inverse()
439  * SE3(p1.matrix(), q1.derived().template head<3>())).toVector();
440  }
441 
443  template <ArgumentPosition arg, class ConfigL_t, class ConfigR_t, class JacobianOut_t>
444  void dDifference_impl (const Eigen::MatrixBase<ConfigL_t> & q0,
445  const Eigen::MatrixBase<ConfigR_t> & q1,
446  const Eigen::MatrixBase<JacobianOut_t>& J) const
447  {
448  ConstQuaternionMap_t p0 (q0.derived().template tail<4>().data());
449  ConstQuaternionMap_t p1 (q1.derived().template tail<4>().data());
450  typename SE3::Matrix3 R0 (p0.matrix()),
451  R1 (p1.matrix());
452  SE3 M ( SE3(R0, q0.derived().template head<3>()).inverse()
453  * SE3(R1, q1.derived().template head<3>()));
454 
455  if (arg == ARG0) {
456  JacobianMatrix_t J1;
457  Jlog6 (M, J1);
458 
459  typename SE3::Vector3 p1_p0 ( q1.derived().template head<3>()
460  - q0.derived().template head<3>());
461 
462  JacobianOut_t& J0 = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J);
463  J0.template topLeftCorner <3,3> ().noalias() = - M.rotation().transpose();
464  J0.template topRightCorner<3,3> ().noalias() = R1.transpose() * skew (p1_p0) * R0;
465  J0.template bottomLeftCorner <3,3> ().setZero();
466  J0.template bottomRightCorner<3,3> ().noalias() = - M.rotation().transpose();
467  J0.applyOnTheLeft(J1);
468  } else if (arg == ARG1) {
469  Jlog6 (M, J);
470  }
471  }
472 
473  template <class ConfigIn_t, class Velocity_t, class ConfigOut_t>
474  static void integrate_impl(const Eigen::MatrixBase<ConfigIn_t> & q,
475  const Eigen::MatrixBase<Velocity_t> & v,
476  const Eigen::MatrixBase<ConfigOut_t> & qout)
477  {
478  ConfigOut_t & out = PINOCCHIO_EIGEN_CONST_CAST(ConfigOut_t,qout);
479  ConstQuaternionMap_t quat(q.derived().template tail<4>().data());
480  QuaternionMap_t res_quat (out.template tail<4>().data());
481 
482  using internal::if_then_else;
483 
484  SE3 M0 (quat.matrix(), q.derived().template head<3>());
485  MotionRef<const Velocity_t> mref_v(v.derived());
486  SE3 M1 (M0 * exp6(mref_v));
487 
488  out.template head<3>() = M1.translation();
489  quaternion::assignQuaternion(res_quat,M1.rotation()); // required by CasADi
490  const Scalar dot_product = res_quat.dot(quat);
491  for(Eigen::DenseIndex k = 0; k < 4; ++k)
492  {
493  res_quat.coeffs().coeffRef(k) = if_then_else(dot_product < 0,
494  -res_quat.coeffs().coeff(k),
495  res_quat.coeffs().coeff(k));
496  }
497 
498  // Norm of qs might be epsilon-different to 1, so M1.rotation might be epsilon-different to a rotation matrix.
499  // It is then safer to re-normalized after converting M1.rotation to quaternion.
501  }
502 
503  template <class Config_t, class Jacobian_t>
504  static void integrateCoeffWiseJacobian_impl(const Eigen::MatrixBase<Config_t> & q,
505  const Eigen::MatrixBase<Jacobian_t> & J)
506  {
507  assert(J.rows() == nq() && J.cols() == nv() && "J is not of the right dimension");
508 
509  typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(Config_t) ConfigPlainType;
510  typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(Jacobian_t) JacobianPlainType;
511  typedef typename ConfigPlainType::Scalar Scalar;
512  Jacobian_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t,J);
513  Jout.setZero();
514 
515  ConstQuaternionMap_t quat_map(q.derived().template tail<4>().data());
516  Jout.template topLeftCorner<3,3>() = quat_map.toRotationMatrix();
517 // Jexp3(quat,Jout.template bottomRightCorner<4,3>());
518 
519  typedef Eigen::Matrix<Scalar,4,3,JacobianPlainType::Options|Eigen::RowMajor> Jacobian43;
521  Jacobian43 Jexp3QuatCoeffWise;
522 
523  Scalar theta;
524  typename SE3::Vector3 v = quaternion::log3(quat_map,theta);
525  quaternion::Jexp3CoeffWise(v,Jexp3QuatCoeffWise);
526  typename SE3::Matrix3 Jlog;
527  Jlog3(theta,v,Jlog);
528 
529 // std::cout << "Jexp3QuatCoeffWise\n" << Jexp3QuatCoeffWise << std::endl;
530 // std::cout << "Jlog\n" << Jlog << std::endl;
531 
532 // if(quat_map.w() >= 0.) // comes from the log3 for quaternions which may change the sign.
533  if(quat_map.coeffs()[3] >= Scalar(0)) // comes from the log3 for quaternions which may change the sign.
534  PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t,J).template bottomRightCorner<4,3>().noalias() = Jexp3QuatCoeffWise * Jlog;
535  else
536  PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t,J).template bottomRightCorner<4,3>().noalias() = -Jexp3QuatCoeffWise * Jlog;
537  }
538 
539  template <class Config_t, class Tangent_t, class JacobianOut_t>
540  static void dIntegrate_dq_impl(const Eigen::MatrixBase<Config_t > & /*q*/,
541  const Eigen::MatrixBase<Tangent_t> & v,
542  const Eigen::MatrixBase<JacobianOut_t>& J)
543  {
544  JacobianOut_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J);
545  Jout = exp6(MotionRef<const Tangent_t>(v.derived())).toDualActionMatrix().transpose();
546  }
547 
548  template <class Config_t, class Tangent_t, class JacobianOut_t>
549  static void dIntegrate_dv_impl(const Eigen::MatrixBase<Config_t > & /*q*/,
550  const Eigen::MatrixBase<Tangent_t> & v,
551  const Eigen::MatrixBase<JacobianOut_t>& J)
552  {
553  Jexp6(MotionRef<const Tangent_t>(v.derived()), J.derived());
554  }
555 
556  // interpolate_impl use default implementation.
557  // template <class ConfigL_t, class ConfigR_t, class ConfigOut_t>
558  // static void interpolate_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
559  // const Eigen::MatrixBase<ConfigR_t> & q1,
560  // const Scalar& u,
561  // const Eigen::MatrixBase<ConfigOut_t>& qout)
562  // {
563  // }
564 
565  template <class ConfigL_t, class ConfigR_t>
566  static Scalar squaredDistance_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
567  const Eigen::MatrixBase<ConfigR_t> & q1)
568  {
569  TangentVector_t t;
570  difference_impl(q0, q1, t);
571  return t.squaredNorm();
572  }
573 
574  template <class Config_t>
575  static void normalize_impl (const Eigen::MatrixBase<Config_t>& qout)
576  {
577  Config_t & qout_ = PINOCCHIO_EIGEN_CONST_CAST(Config_t,qout);
578  qout_.template tail<4>().normalize();
579  }
580 
581  template <class Config_t>
582  void random_impl (const Eigen::MatrixBase<Config_t>& qout) const
583  {
584  R3crossSO3_t().random(qout);
585  }
586 
587  template <class ConfigL_t, class ConfigR_t, class ConfigOut_t>
588  void randomConfiguration_impl(const Eigen::MatrixBase<ConfigL_t> & lower,
589  const Eigen::MatrixBase<ConfigR_t> & upper,
590  const Eigen::MatrixBase<ConfigOut_t> & qout)
591  const
592  {
593  R3crossSO3_t ().randomConfiguration(lower, upper, qout);
594  }
595 
596  template <class ConfigL_t, class ConfigR_t>
597  static bool isSameConfiguration_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
598  const Eigen::MatrixBase<ConfigR_t> & q1,
599  const Scalar & prec)
600  {
601  return R3crossSO3_t().isSameConfiguration(q0, q1, prec);
602  }
603  }; // struct SpecialEuclideanOperationTpl<3>
604 
605 } // namespace pinocchio
606 
607 #endif // ifndef __pinocchio_special_euclidean_operation_hpp__
int nv(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointNvVisitor to get the dimension of the joint tangent space...
static Index nv()
Get dimension of Lie Group tangent space.
static Index nv()
Get dimension of Lie Group tangent space.
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.
Definition: explog.hpp:168
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 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.
std::string name(const LieGroupGenericTpl< LieGroupCollection > &lg)
Visit a LieGroupVariant to get the name of it.
void firstOrderNormalize(const Eigen::QuaternionBase< D > &q)
Definition: quaternion.hpp:82
void Jlog6(const SE3Tpl< Scalar, Options > &M, const Eigen::MatrixBase< Matrix6Like > &Jlog)
Derivative of log6 where and .
Definition: explog.hpp:396
MotionTpl< Scalar, Options > log6(const SE3Tpl< Scalar, Options > &M)
Log: SE3 -> se3.
Definition: explog.hpp:288
void Jexp3CoeffWise(const Eigen::MatrixBase< Vector3Like > &v, const Eigen::MatrixBase< Matrix43Like > &Jexp)
Derivative of where is a small perturbation of at identity.
void Jexp6(const MotionDense< MotionDerived > &nu, const Eigen::MatrixBase< Matrix6Like > &Jexp)
Derivative of exp6 Computed as the inverse of Jlog6.
Definition: explog.hpp:324
void forwardKinematics(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q)
Update the joint placements according to the current joint configuration.
void SINCOS(const S1 &a, S2 *sa, S3 *ca)
Computes sin/cos values of a given input scalar.
Definition: sincos.hpp:26
Main pinocchio namespace.
Definition: treeview.dox:24
void neutral(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ReturnType > &qout)
Return the neutral configuration element related to the model configuration space.
Common traits structure to fully define base classes for CRTP.
Definition: fwd.hpp:35
void skew(const Eigen::MatrixBase< Vector3 > &v, const Eigen::MatrixBase< Matrix3 > &M)
Computes the skew representation of a given 3d vector, i.e. the antisymmetric matrix representation o...
Definition: skew.hpp:21
void normalize(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorType > &qout)
Normalize a configuration vector.
SE3Tpl< typename MotionDerived::Scalar, typename MotionDerived::Vector3::Options > exp6(const MotionDense< MotionDerived > &nu)
Exp: se3 -> SE3.
Definition: explog.hpp:211