pinocchio  2.4.4
A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
special-euclidean.hpp
1 //
2 // Copyright (c) 2016-2020 CNRS INRIA
3 //
4 
5 #ifndef __pinocchio_multibody_liegroup_special_euclidean_operation_hpp__
6 #define __pinocchio_multibody_liegroup_special_euclidean_operation_hpp__
7 
8 #include <limits>
9 
10 #include "pinocchio/macros.hpp"
11 #include "pinocchio/math/quaternion.hpp"
12 #include "pinocchio/spatial/fwd.hpp"
13 #include "pinocchio/utils/static-if.hpp"
14 #include "pinocchio/spatial/se3.hpp"
15 #include "pinocchio/multibody/liegroup/liegroup-base.hpp"
16 
17 #include "pinocchio/multibody/liegroup/vector-space.hpp"
18 #include "pinocchio/multibody/liegroup/cartesian-product.hpp"
19 #include "pinocchio/multibody/liegroup/special-orthogonal.hpp"
20 
21 namespace pinocchio
22 {
23  template<int Dim, typename Scalar, int Options = 0>
24  struct SpecialEuclideanOperationTpl
25  {};
26 
27  template<int Dim, typename Scalar, int Options>
28  struct traits< SpecialEuclideanOperationTpl<Dim,Scalar,Options> >
29  {};
30 
31  template<typename _Scalar, int _Options>
32  struct traits< SpecialEuclideanOperationTpl<2,_Scalar,_Options> >
33  {
34  typedef _Scalar Scalar;
35  enum
36  {
37  Options = _Options,
38  NQ = 4,
39  NV = 3
40  };
41  };
42 
43  // SE(2)
44  template<typename _Scalar, int _Options>
45  struct SpecialEuclideanOperationTpl<2,_Scalar,_Options>
46  : public LieGroupBase <SpecialEuclideanOperationTpl<2,_Scalar,_Options> >
47  {
48  PINOCCHIO_LIE_GROUP_TPL_PUBLIC_INTERFACE(SpecialEuclideanOperationTpl);
49 
50  typedef VectorSpaceOperationTpl<2,Scalar,Options> R2_t;
51  typedef SpecialOrthogonalOperationTpl<2,Scalar,Options> SO2_t;
52  typedef CartesianProductOperation<R2_t, SO2_t> R2crossSO2_t;
53 
54  typedef Eigen::Matrix<Scalar,2,2,Options> Matrix2;
55  typedef Eigen::Matrix<Scalar,2,1,Options> Vector2;
56 
57  template<typename TangentVector, typename Matrix2Like, typename Vector2Like>
58  static void exp(const Eigen::MatrixBase<TangentVector> & v,
59  const Eigen::MatrixBase<Matrix2Like> & R,
60  const Eigen::MatrixBase<Vector2Like> & t)
61  {
62  EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(TangentVector_t,TangentVector);
63  EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix2Like, 2, 2);
64  EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector2Like, 2);
65 
66  typedef typename Matrix2Like::Scalar Scalar;
67  const Scalar omega = v(2);
68  Scalar cv,sv; SINCOS(omega, &sv, &cv);
69  PINOCCHIO_EIGEN_CONST_CAST(Matrix2Like,R) << cv, -sv, sv, cv;
70  using internal::if_then_else;
71 
72  {
73  typename PINOCCHIO_EIGEN_PLAIN_TYPE(Vector2Like) vcross(-v(1), v(0));
74  vcross -= -v(1)*R.col(0) + v(0)*R.col(1);
75  vcross /= omega;
76  Scalar omega_abs = math::fabs(omega);
77  PINOCCHIO_EIGEN_CONST_CAST(Vector2Like,t).coeffRef(0) = if_then_else(internal::GT, omega_abs , Scalar(1e-14),
78  vcross.coeff(0),
79  v.coeff(0));
80 
81  PINOCCHIO_EIGEN_CONST_CAST(Vector2Like,t).coeffRef(1) = if_then_else(internal::GT, omega_abs, Scalar(1e-14),
82  vcross.coeff(1),
83  v.coeff(1));
84  }
85 
86  }
87 
88  template<typename Matrix2Like, typename Vector2Like, typename Matrix3Like>
89  static void toInverseActionMatrix(const Eigen::MatrixBase<Matrix2Like> & R,
90  const Eigen::MatrixBase<Vector2Like> & t,
91  const Eigen::MatrixBase<Matrix3Like> & M,
92  const AssignmentOperatorType op)
93  {
94  EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix2Like, 2, 2);
95  EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector2Like, 2);
96  EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix3Like, 3, 3);
97  Matrix3Like & Mout = PINOCCHIO_EIGEN_CONST_CAST(Matrix3Like,M);
98  typedef typename Matrix3Like::Scalar Scalar;
99 
100  typename PINOCCHIO_EIGEN_PLAIN_TYPE(Vector2Like) tinv((R.transpose() * t).reverse());
101  tinv[0] *= Scalar(-1.);
102  switch(op)
103  {
104  case SETTO:
105  Mout.template topLeftCorner<2,2>() = R.transpose();
106  Mout.template topRightCorner<2,1>() = tinv;
107  Mout.template bottomLeftCorner<1,2>().setZero();
108  Mout(2,2) = (Scalar)1;
109  break;
110  case ADDTO:
111  Mout.template topLeftCorner<2,2>() += R.transpose();
112  Mout.template topRightCorner<2,1>() += tinv;
113  Mout(2,2) += (Scalar)1;
114  break;
115  case RMTO:
116  Mout.template topLeftCorner<2,2>() -= R.transpose();
117  Mout.template topRightCorner<2,1>() -= tinv;
118  Mout(2,2) -= (Scalar)1;
119  break;
120  default:
121  assert(false && "Wrong Op requesed value");
122  break;
123  }
124 
125 
126 
127  }
128 
129  template<typename Matrix2Like, typename Vector2Like, typename TangentVector>
130  static void log(const Eigen::MatrixBase<Matrix2Like> & R,
131  const Eigen::MatrixBase<Vector2Like> & p,
132  const Eigen::MatrixBase<TangentVector> & v)
133  {
134  EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix2Like, 2, 2);
135  EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector2Like, 2);
136  EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(TangentVector_t,TangentVector);
137 
138  TangentVector & vout = PINOCCHIO_EIGEN_CONST_CAST(TangentVector,v);
139 
140  typedef typename Matrix2Like::Scalar Scalar1;
141 
142  Scalar1 t = SO2_t::log(R);
143  const Scalar1 tabs = math::fabs(t);
144  const Scalar1 t2 = t*t;
145  Scalar1 st,ct; SINCOS(tabs, &st, &ct);
146  Scalar1 alpha;
147  alpha = internal::if_then_else(internal::LT, tabs, Scalar(1e-4),
148  1 - t2/12 - t2*t2/720,
149  tabs*st/(2*(1-ct)));
150 
151  vout.template head<2>().noalias() = alpha * p;
152  vout(0) += t/2 * p(1);
153  vout(1) += -t/2 * p(0);
154  vout(2) = t;
155  }
156 
157  template<typename Matrix2Like, typename Vector2Like, typename JacobianOutLike>
158  static void Jlog(const Eigen::MatrixBase<Matrix2Like> & R,
159  const Eigen::MatrixBase<Vector2Like> & p,
160  const Eigen::MatrixBase<JacobianOutLike> & J)
161  {
162  EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix2Like, 2, 2);
163  EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector2Like, 2);
164  EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(JacobianOutLike, JacobianMatrix_t);
165 
166  JacobianOutLike & Jout = PINOCCHIO_EIGEN_CONST_CAST(JacobianOutLike,J);
167 
168  typedef typename Matrix2Like::Scalar Scalar1;
169 
170  Scalar1 t = SO2_t::log(R);
171  const Scalar1 tabs = math::fabs(t);
172  Scalar1 alpha, alpha_dot;
173  Scalar1 t2 = t*t;
174  Scalar1 st,ct; SINCOS(t, &st, &ct);
175  Scalar1 inv_2_1_ct = 0.5 / (1-ct);
176 
177  alpha = internal::if_then_else(internal::LT, tabs, Scalar(1e-4),
178  1 - t2/12,
179  t*st*inv_2_1_ct);
180  alpha_dot = internal::if_then_else(internal::LT, tabs, Scalar(1e-4),
181  - t / 6 - t2*t / 180,
182  (st-t) * inv_2_1_ct);
183 
184  typename PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix2Like) V;
185  V(0,0) = V(1,1) = alpha;
186  V(1,0) = - t / 2;
187  V(0,1) = - V(1,0);
188 
189  Jout.template topLeftCorner <2,2>().noalias() = V * R;
190  Jout.template topRightCorner<2,1>() << alpha_dot*p[0] + p[1]/2, -p(0)/2 + alpha_dot*p(1);
191  Jout.template bottomLeftCorner<1,2>().setZero();
192  Jout(2,2) = 1;
193  }
194 
199  static Index nq()
200  {
201  return NQ;
202  }
204  static Index nv()
205  {
206  return NV;
207  }
208 
209  static ConfigVector_t neutral()
210  {
211  ConfigVector_t n; n << Scalar(0), Scalar(0), Scalar(1), Scalar(0);
212  return n;
213  }
214 
215  static std::string name()
216  {
217  return std::string("SE(2)");
218  }
219 
220  template <class ConfigL_t, class ConfigR_t, class Tangent_t>
221  static void difference_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
222  const Eigen::MatrixBase<ConfigR_t> & q1,
223  const Eigen::MatrixBase<Tangent_t> & d)
224  {
225  Matrix2 R0, R1; Vector2 t0, t1;
226  forwardKinematics(R0, t0, q0);
227  forwardKinematics(R1, t1, q1);
228  Matrix2 R (R0.transpose() * R1);
229  Vector2 t (R0.transpose() * (t1 - t0));
230 
231  log(R, t, d);
232  }
233 
234  template <ArgumentPosition arg, class ConfigL_t, class ConfigR_t, class JacobianOut_t>
235  void dDifference_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
236  const Eigen::MatrixBase<ConfigR_t> & q1,
237  const Eigen::MatrixBase<JacobianOut_t>& J) const
238  {
239  Matrix2 R0, R1; Vector2 t0, t1;
240  forwardKinematics(R0, t0, q0);
241  forwardKinematics(R1, t1, q1);
242  Matrix2 R (R0.transpose() * R1);
243  Vector2 t (R0.transpose() * (t1 - t0));
244 
245  if (arg == ARG0) {
246  JacobianMatrix_t J1;
247  Jlog (R, t, J1);
248 
249  // pcross = [ y1-y0, - (x1 - x0) ]
250  Vector2 pcross (q1(1) - q0(1), q0(0) - q1(0));
251 
252  JacobianOut_t& J0 = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, J);
253  J0.template topLeftCorner <2,2> ().noalias() = - R.transpose();
254  J0.template topRightCorner<2,1> ().noalias() = R1.transpose() * pcross;
255  J0.template bottomLeftCorner <1,2> ().setZero();
256  J0 (2,2) = -1;
257  J0.applyOnTheLeft(J1);
258  } else if (arg == ARG1) {
259  Jlog (R, t, J);
260  }
261  }
262 
263  template <class ConfigIn_t, class Velocity_t, class ConfigOut_t>
264  static void integrate_impl(const Eigen::MatrixBase<ConfigIn_t> & q,
265  const Eigen::MatrixBase<Velocity_t> & v,
266  const Eigen::MatrixBase<ConfigOut_t> & qout)
267  {
268  ConfigOut_t & out = PINOCCHIO_EIGEN_CONST_CAST(ConfigOut_t,qout);
269 
270  Matrix2 R0, R;
271  Vector2 t0, t;
272  forwardKinematics(R0, t0, q);
273  exp(v, R, t);
274 
275  out.template head<2>().noalias() = R0 * t + t0;
276  out.template tail<2>().noalias() = R0 * R.col(0);
277  }
278 
279  template <class Config_t, class Jacobian_t>
280  static void integrateCoeffWiseJacobian_impl(const Eigen::MatrixBase<Config_t> & q,
281  const Eigen::MatrixBase<Jacobian_t> & J)
282  {
283  assert(J.rows() == nq() && J.cols() == nv() && "J is not of the right dimension");
284 
285  Jacobian_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t,J);
286  Jout.setZero();
287 
288  const typename Config_t::Scalar & c_theta = q(2),
289  & s_theta = q(3);
290 
291  Jout.template topLeftCorner<2,2>() << c_theta, -s_theta, s_theta, c_theta;
292  Jout.template bottomRightCorner<2,1>() << -s_theta, c_theta;
293  }
294 
295  template <class Config_t, class Tangent_t, class JacobianOut_t>
296  static void dIntegrate_dq_impl(const Eigen::MatrixBase<Config_t > & /*q*/,
297  const Eigen::MatrixBase<Tangent_t> & v,
298  const Eigen::MatrixBase<JacobianOut_t>& J,
299  const AssignmentOperatorType op=SETTO)
300  {
301  JacobianOut_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J);
302 
303  Matrix2 R;
304  Vector2 t;
305  exp(v, R, t);
306 
307  toInverseActionMatrix(R, t, Jout, op);
308  }
309 
310  template <class Config_t, class Tangent_t, class JacobianOut_t>
311  static void dIntegrate_dv_impl(const Eigen::MatrixBase<Config_t > & /*q*/,
312  const Eigen::MatrixBase<Tangent_t> & v,
313  const Eigen::MatrixBase<JacobianOut_t> & J,
314  const AssignmentOperatorType op=SETTO)
315  {
316  JacobianOut_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J);
317  // TODO sparse version
318  MotionTpl<Scalar,0> nu; nu.toVector() << v.template head<2>(), 0, 0, 0, v[2];
319  Eigen::Matrix<Scalar,6,6> Jtmp6;
320  Jexp6(nu, Jtmp6);
321 
322  switch(op)
323  {
324  case SETTO:
325  Jout << Jtmp6.template topLeftCorner<2,2>(), Jtmp6.template topRightCorner<2,1>(),
326  Jtmp6.template bottomLeftCorner<1,2>(), Jtmp6.template bottomRightCorner<1,1>();
327  break;
328  case ADDTO:
329  Jout.template topLeftCorner<2,2>() += Jtmp6.template topLeftCorner<2,2>();
330  Jout.template topRightCorner<2,1>() += Jtmp6.template topRightCorner<2,1>();
331  Jout.template bottomLeftCorner<1,2>() += Jtmp6.template bottomLeftCorner<1,2>();
332  Jout.template bottomRightCorner<1,1>() += Jtmp6.template bottomRightCorner<1,1>();
333  break;
334  case RMTO:
335  Jout.template topLeftCorner<2,2>() -= Jtmp6.template topLeftCorner<2,2>();
336  Jout.template topRightCorner<2,1>() -= Jtmp6.template topRightCorner<2,1>();
337  Jout.template bottomLeftCorner<1,2>() -= Jtmp6.template bottomLeftCorner<1,2>();
338  Jout.template bottomRightCorner<1,1>() -= Jtmp6.template bottomRightCorner<1,1>();
339  break;
340  default:
341  assert(false && "Wrong Op requesed value");
342  break;
343  }
344  }
345 
346  template <class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
347  void dIntegrateTransport_dq_impl(const Eigen::MatrixBase<Config_t > & /*q*/,
348  const Eigen::MatrixBase<Tangent_t> & v,
349  const Eigen::MatrixBase<JacobianIn_t> & Jin,
350  const Eigen::MatrixBase<JacobianOut_t> & J_out) const
351  {
352  JacobianOut_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J_out);
353  Matrix2 R;
354  Vector2 t;
355  exp(v, R, t);
356 
357  Vector2 tinv = (R.transpose() * t).reverse();
358  tinv[0] *= Scalar(-1.);
359 
360  Jout.template topRows<2>().noalias() = R.transpose() * Jin.template topRows<2>();
361  Jout.template topRows<2>().noalias() += tinv * Jin.template bottomRows<1>();
362  Jout.template bottomRows<1>() = Jin.template bottomRows<1>();
363  }
364 
365  template <class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
366  void dIntegrateTransport_dv_impl(const Eigen::MatrixBase<Config_t > & /*q*/,
367  const Eigen::MatrixBase<Tangent_t> & v,
368  const Eigen::MatrixBase<JacobianIn_t> & Jin,
369  const Eigen::MatrixBase<JacobianOut_t> & J_out) const
370  {
371  JacobianOut_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J_out);
372  MotionTpl<Scalar,0> nu; nu.toVector() << v.template head<2>(), 0, 0, 0, v[2];
373 
374  Eigen::Matrix<Scalar,6,6> Jtmp6;
375  Jexp6(nu, Jtmp6);
376 
377  Jout.template topRows<2>().noalias()
378  = Jtmp6.template topLeftCorner<2,2>() * Jin.template topRows<2>();
379  Jout.template topRows<2>().noalias()
380  += Jtmp6.template topRightCorner<2,1>() * Jin.template bottomRows<1>();
381  Jout.template bottomRows<1>().noalias()
382  = Jtmp6.template bottomLeftCorner<1,2>()* Jin.template topRows<2>();
383  Jout.template bottomRows<1>().noalias()
384  += Jtmp6.template bottomRightCorner<1,1>() * Jin.template bottomRows<1>();
385 
386  }
387 
388  template <class Config_t, class Tangent_t, class Jacobian_t>
389  void dIntegrateTransport_dq_impl(const Eigen::MatrixBase<Config_t > & /*q*/,
390  const Eigen::MatrixBase<Tangent_t> & v,
391  const Eigen::MatrixBase<Jacobian_t> & J) const
392  {
393  Jacobian_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t,J);
394  Matrix2 R;
395  Vector2 t;
396  exp(v, R, t);
397 
398  Vector2 tinv = (R.transpose() * t).reverse();
399  tinv[0] *= Scalar(-1);
400  //TODO: Aliasing
401  Jout.template topRows<2>() = R.transpose() * Jout.template topRows<2>();
402  //No Aliasing
403  Jout.template topRows<2>().noalias() += tinv * Jout.template bottomRows<1>();
404  }
405 
406  template <class Config_t, class Tangent_t, class Jacobian_t>
407  void dIntegrateTransport_dv_impl(const Eigen::MatrixBase<Config_t > & /*q*/,
408  const Eigen::MatrixBase<Tangent_t> & v,
409  const Eigen::MatrixBase<Jacobian_t> & J) const
410  {
411  Jacobian_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t,J);
412  MotionTpl<Scalar,0> nu; nu.toVector() << v.template head<2>(), 0, 0, 0, v[2];
413 
414  Eigen::Matrix<Scalar,6,6> Jtmp6;
415  Jexp6(nu, Jtmp6);
416  //TODO: Remove aliasing
417  Jout.template topRows<2>()
418  = Jtmp6.template topLeftCorner<2,2>() * Jout.template topRows<2>();
419  Jout.template topRows<2>().noalias()
420  += Jtmp6.template topRightCorner<2,1>() * Jout.template bottomRows<1>();
421  Jout.template bottomRows<1>()
422  = Jtmp6.template bottomRightCorner<1,1>() * Jout.template bottomRows<1>();
423  Jout.template bottomRows<1>().noalias()
424  += Jtmp6.template bottomLeftCorner<1,2>() * Jout.template topRows<2>();
425  }
426 
427  template <class Config_t>
428  static void normalize_impl(const Eigen::MatrixBase<Config_t> & qout)
429  {
430  PINOCCHIO_EIGEN_CONST_CAST(Config_t,qout).template tail<2>().normalize();
431  }
432 
433  template <class Config_t>
434  void random_impl(const Eigen::MatrixBase<Config_t> & qout) const
435  {
436  R2crossSO2_t().random(qout);
437  }
438 
439  template <class ConfigL_t, class ConfigR_t, class ConfigOut_t>
440  void randomConfiguration_impl(const Eigen::MatrixBase<ConfigL_t> & lower,
441  const Eigen::MatrixBase<ConfigR_t> & upper,
442  const Eigen::MatrixBase<ConfigOut_t> & qout)
443  const
444  {
445  R2crossSO2_t ().randomConfiguration(lower, upper, qout);
446  }
447 
448  template <class ConfigL_t, class ConfigR_t>
449  static bool isSameConfiguration_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
450  const Eigen::MatrixBase<ConfigR_t> & q1,
451  const Scalar & prec)
452  {
453  return R2crossSO2_t().isSameConfiguration(q0, q1, prec);
454  }
455 
456  protected:
457 
458  template<typename Matrix2Like, typename Vector2Like, typename Vector4Like>
459  static void forwardKinematics(const Eigen::MatrixBase<Matrix2Like> & R,
460  const Eigen::MatrixBase<Vector2Like> & t,
461  const Eigen::MatrixBase<Vector4Like> & q)
462  {
463  EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(Matrix2Like, Matrix2);
464  EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(Vector2Like, Vector2);
465  EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigVector_t,Vector4Like);
466 
467  PINOCCHIO_EIGEN_CONST_CAST(Vector2Like,t) = q.template head<2>();
468  const typename Vector4Like::Scalar & c_theta = q(2),
469  & s_theta = q(3);
470  PINOCCHIO_EIGEN_CONST_CAST(Matrix2Like,R) << c_theta, -s_theta, s_theta, c_theta;
471 
472  }
473  }; // struct SpecialEuclideanOperationTpl<2>
474 
475  template<typename _Scalar, int _Options>
476  struct traits< SpecialEuclideanOperationTpl<3,_Scalar,_Options> >
477  {
478  typedef _Scalar Scalar;
479  enum
480  {
481  Options = _Options,
482  NQ = 7,
483  NV = 6
484  };
485  };
486 
488  template<typename _Scalar, int _Options>
489  struct SpecialEuclideanOperationTpl<3,_Scalar,_Options>
490  : public LieGroupBase <SpecialEuclideanOperationTpl<3,_Scalar,_Options> >
491  {
492  PINOCCHIO_LIE_GROUP_TPL_PUBLIC_INTERFACE(SpecialEuclideanOperationTpl);
493 
494  typedef CartesianProductOperation <VectorSpaceOperationTpl<3,Scalar,Options>, SpecialOrthogonalOperationTpl<3,Scalar,Options> > R3crossSO3_t;
495 
496  typedef Eigen::Quaternion<Scalar,Options> Quaternion_t;
497  typedef Eigen::Map< Quaternion_t> QuaternionMap_t;
498  typedef Eigen::Map<const Quaternion_t> ConstQuaternionMap_t;
499  typedef SE3Tpl<Scalar,Options> Transformation_t;
500  typedef SE3Tpl<Scalar,Options> SE3;
501  typedef typename Eigen::NumTraits<Scalar>::Real RealScalar;
502 
507  static Index nq()
508  {
509  return NQ;
510  }
512  static Index nv()
513  {
514  return NV;
515  }
516 
517  static ConfigVector_t neutral()
518  {
519  ConfigVector_t n; n.template head<6>().setZero(); n[6] = 1;
520  return n;
521  }
522 
523  static std::string name()
524  {
525  return std::string("SE(3)");
526  }
527 
528  template <class ConfigL_t, class ConfigR_t, class Tangent_t>
529  static void difference_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
530  const Eigen::MatrixBase<ConfigR_t> & q1,
531  const Eigen::MatrixBase<Tangent_t> & d)
532  {
533  ConstQuaternionMap_t quat0 (q0.derived().template tail<4>().data());
534  assert(quaternion::isNormalized(quat0,RealScalar(PINOCCHIO_DEFAULT_QUATERNION_NORM_TOLERANCE_VALUE)));
535  ConstQuaternionMap_t quat1 (q1.derived().template tail<4>().data());
536  assert(quaternion::isNormalized(quat1,RealScalar(PINOCCHIO_DEFAULT_QUATERNION_NORM_TOLERANCE_VALUE)));
537 
538  PINOCCHIO_EIGEN_CONST_CAST(Tangent_t,d)
539  = log6( SE3(quat0.matrix(), q0.derived().template head<3>()).inverse()
540  * SE3(quat1.matrix(), q1.derived().template head<3>())).toVector();
541  }
542 
544  template <ArgumentPosition arg, class ConfigL_t, class ConfigR_t, class JacobianOut_t>
545  void dDifference_impl (const Eigen::MatrixBase<ConfigL_t> & q0,
546  const Eigen::MatrixBase<ConfigR_t> & q1,
547  const Eigen::MatrixBase<JacobianOut_t> & J) const
548  {
549  typedef typename SE3::Vector3 Vector3;
550  typedef typename SE3::Matrix3 Matrix3;
551 
552  ConstQuaternionMap_t quat0 (q0.derived().template tail<4>().data());
553  assert(quaternion::isNormalized(quat0,RealScalar(PINOCCHIO_DEFAULT_QUATERNION_NORM_TOLERANCE_VALUE)));
554  ConstQuaternionMap_t quat1 (q1.derived().template tail<4>().data());
555  assert(quaternion::isNormalized(quat1,RealScalar(PINOCCHIO_DEFAULT_QUATERNION_NORM_TOLERANCE_VALUE)));
556 
557  Matrix3 R0(quat0.matrix()), R1 (quat1.matrix());
558  assert(isUnitary(R0)); assert(isUnitary(R1));
559 
560  const SE3 M ( SE3(R0, q0.template head<3>()).inverse()
561  * SE3(R1, q1.template head<3>()));
562 
563  if (arg == ARG0) {
564  JacobianMatrix_t J1;
565  Jlog6 (M, J1);
566 
567  const Vector3 p1_p0 = R1.transpose()*(q1.template head<3>() - q0.template head<3>());
568 
569  JacobianOut_t & J0 = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J);
570  J0.template bottomRightCorner<3,3> ().noalias() = J0.template topLeftCorner <3,3> ().noalias() = - M.rotation().transpose();
571  J0.template topRightCorner<3,3> ().noalias() = skew(p1_p0) * M.rotation().transpose(); // = R1.T * skew(q1_t - q0_t) * R0;
572  J0.template bottomLeftCorner<3,3> ().setZero();
573  J0.applyOnTheLeft(J1);
574  }
575  else if (arg == ARG1) {
576  Jlog6 (M, J);
577  }
578  }
579 
580  template <class ConfigIn_t, class Velocity_t, class ConfigOut_t>
581  static void integrate_impl(const Eigen::MatrixBase<ConfigIn_t> & q,
582  const Eigen::MatrixBase<Velocity_t> & v,
583  const Eigen::MatrixBase<ConfigOut_t> & qout)
584  {
585  ConfigOut_t & out = PINOCCHIO_EIGEN_CONST_CAST(ConfigOut_t,qout);
586  ConstQuaternionMap_t quat(q.derived().template tail<4>().data());
587  assert(quaternion::isNormalized(quat,RealScalar(PINOCCHIO_DEFAULT_QUATERNION_NORM_TOLERANCE_VALUE)));
588  QuaternionMap_t res_quat (out.template tail<4>().data());
589 
590  using internal::if_then_else;
591 
592  SE3 M0 (quat.matrix(), q.derived().template head<3>());
593  MotionRef<const Velocity_t> mref_v(v.derived());
594  SE3 M1 (M0 * exp6(mref_v));
595 
596  out.template head<3>() = M1.translation();
597  quaternion::assignQuaternion(res_quat,M1.rotation()); // required by CasADi
598  const Scalar dot_product = res_quat.dot(quat);
599  for(Eigen::DenseIndex k = 0; k < 4; ++k)
600  {
601  res_quat.coeffs().coeffRef(k) = if_then_else(internal::LT, dot_product, Scalar(0),
602  -res_quat.coeffs().coeff(k),
603  res_quat.coeffs().coeff(k));
604  }
605 
606  // Norm of qs might be epsilon-different to 1, so M1.rotation might be epsilon-different to a rotation matrix.
607  // It is then safer to re-normalized after converting M1.rotation to quaternion.
608  quaternion::firstOrderNormalize(res_quat);
609  assert(quaternion::isNormalized(res_quat,RealScalar(PINOCCHIO_DEFAULT_QUATERNION_NORM_TOLERANCE_VALUE)));
610  }
611 
612  template <class Config_t, class Jacobian_t>
613  static void integrateCoeffWiseJacobian_impl(const Eigen::MatrixBase<Config_t> & q,
614  const Eigen::MatrixBase<Jacobian_t> & J)
615  {
616  assert(J.rows() == nq() && J.cols() == nv() && "J is not of the right dimension");
617 
618  typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(Config_t) ConfigPlainType;
619  typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(Jacobian_t) JacobianPlainType;
620  typedef typename ConfigPlainType::Scalar Scalar;
621 
622  Jacobian_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t,J);
623  Jout.setZero();
624 
625  ConstQuaternionMap_t quat_map(q.derived().template tail<4>().data());
626  assert(quaternion::isNormalized(quat_map,RealScalar(PINOCCHIO_DEFAULT_QUATERNION_NORM_TOLERANCE_VALUE)));
627  Jout.template topLeftCorner<3,3>() = quat_map.toRotationMatrix();
628 // Jexp3(quat,Jout.template bottomRightCorner<4,3>());
629 
630  typedef Eigen::Matrix<Scalar,4,3,JacobianPlainType::Options|Eigen::RowMajor> Jacobian43;
631  typedef SE3Tpl<Scalar,ConfigPlainType::Options> SE3;
632  Jacobian43 Jexp3QuatCoeffWise;
633 
634  Scalar theta;
635  typename SE3::Vector3 v = quaternion::log3(quat_map,theta);
636  quaternion::Jexp3CoeffWise(v,Jexp3QuatCoeffWise);
637  typename SE3::Matrix3 Jlog;
638  Jlog3(theta,v,Jlog);
639 
640 // std::cout << "Jexp3QuatCoeffWise\n" << Jexp3QuatCoeffWise << std::endl;
641 // std::cout << "Jlog\n" << Jlog << std::endl;
642 
643 // if(quat_map.w() >= 0.) // comes from the log3 for quaternions which may change the sign.
644  if(quat_map.coeffs()[3] >= Scalar(0)) // comes from the log3 for quaternions which may change the sign.
645  PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t,J).template bottomRightCorner<4,3>().noalias() = Jexp3QuatCoeffWise * Jlog;
646  else
647  PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t,J).template bottomRightCorner<4,3>().noalias() = -Jexp3QuatCoeffWise * Jlog;
648  }
649 
650  template <class Config_t, class Tangent_t, class JacobianOut_t>
651  static void dIntegrate_dq_impl(const Eigen::MatrixBase<Config_t > & /*q*/,
652  const Eigen::MatrixBase<Tangent_t> & v,
653  const Eigen::MatrixBase<JacobianOut_t>& J,
654  const AssignmentOperatorType op=SETTO)
655  {
656  JacobianOut_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J);
657 
658  switch(op)
659  {
660  case SETTO:
661  Jout = exp6(MotionRef<const Tangent_t>(v.derived())).toDualActionMatrix().transpose();
662  break;
663  case ADDTO:
664  Jout += exp6(MotionRef<const Tangent_t>(v.derived())).toDualActionMatrix().transpose();
665  break;
666  case RMTO:
667  Jout -= exp6(MotionRef<const Tangent_t>(v.derived())).toDualActionMatrix().transpose();
668  break;
669  default:
670  assert(false && "Wrong Op requesed value");
671  break;
672  }
673  }
674 
675  template <class Config_t, class Tangent_t, class JacobianOut_t>
676  static void dIntegrate_dv_impl(const Eigen::MatrixBase<Config_t > & /*q*/,
677  const Eigen::MatrixBase<Tangent_t> & v,
678  const Eigen::MatrixBase<JacobianOut_t>& J,
679  const AssignmentOperatorType op=SETTO)
680  {
681  switch(op)
682  {
683  case SETTO:
684  Jexp6<SETTO>(MotionRef<const Tangent_t>(v.derived()), J.derived());
685  break;
686  case ADDTO:
687  Jexp6<ADDTO>(MotionRef<const Tangent_t>(v.derived()), J.derived());
688  break;
689  case RMTO:
690  Jexp6<RMTO>(MotionRef<const Tangent_t>(v.derived()), J.derived());
691  break;
692  default:
693  assert(false && "Wrong Op requesed value");
694  break;
695  }
696  }
697 
698  template <class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
699  void dIntegrateTransport_dq_impl(const Eigen::MatrixBase<Config_t > & /*q*/,
700  const Eigen::MatrixBase<Tangent_t> & v,
701  const Eigen::MatrixBase<JacobianIn_t> & Jin,
702  const Eigen::MatrixBase<JacobianOut_t> & J_out) const
703  {
704  JacobianOut_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J_out);
705  Eigen::Matrix<Scalar,6,6> Jtmp6;
706  Jtmp6 = exp6(MotionRef<const Tangent_t>(v.derived())).toDualActionMatrix().transpose();
707 
708  Jout.template topRows<3>().noalias()
709  = Jtmp6.template topLeftCorner<3,3>() * Jin.template topRows<3>();
710  Jout.template topRows<3>().noalias()
711  += Jtmp6.template topRightCorner<3,3>() * Jin.template bottomRows<3>();
712  Jout.template bottomRows<3>().noalias()
713  = Jtmp6.template bottomRightCorner<3,3>() * Jin.template bottomRows<3>();
714  }
715 
716  template <class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
717  void dIntegrateTransport_dv_impl(const Eigen::MatrixBase<Config_t > & /*q*/,
718  const Eigen::MatrixBase<Tangent_t> & v,
719  const Eigen::MatrixBase<JacobianIn_t> & Jin,
720  const Eigen::MatrixBase<JacobianOut_t> & J_out) const
721  {
722  JacobianOut_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J_out);
723  Eigen::Matrix<Scalar,6,6> Jtmp6;
724  Jexp6<SETTO>(MotionRef<const Tangent_t>(v.derived()), Jtmp6);
725 
726  Jout.template topRows<3>().noalias()
727  = Jtmp6.template topLeftCorner<3,3>() * Jin.template topRows<3>();
728  Jout.template topRows<3>().noalias()
729  += Jtmp6.template topRightCorner<3,3>() * Jin.template bottomRows<3>();
730  Jout.template bottomRows<3>().noalias()
731  = Jtmp6.template bottomRightCorner<3,3>() * Jin.template bottomRows<3>();
732  }
733 
734 
735  template <class Config_t, class Tangent_t, class Jacobian_t>
736  void dIntegrateTransport_dq_impl(const Eigen::MatrixBase<Config_t > & /*q*/,
737  const Eigen::MatrixBase<Tangent_t> & v,
738  const Eigen::MatrixBase<Jacobian_t> & J_out) const
739  {
740  Jacobian_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t,J_out);
741  Eigen::Matrix<Scalar,6,6> Jtmp6;
742  Jtmp6 = exp6(MotionRef<const Tangent_t>(v.derived())).toDualActionMatrix().transpose();
743 
744  //Aliasing
745  Jout.template topRows<3>()
746  = Jtmp6.template topLeftCorner<3,3>() * Jout.template topRows<3>();
747  Jout.template topRows<3>().noalias()
748  += Jtmp6.template topRightCorner<3,3>() * Jout.template bottomRows<3>();
749  Jout.template bottomRows<3>()
750  = Jtmp6.template bottomRightCorner<3,3>() * Jout.template bottomRows<3>();
751  }
752 
753  template <class Config_t, class Tangent_t, class Jacobian_t>
754  void dIntegrateTransport_dv_impl(const Eigen::MatrixBase<Config_t > & /*q*/,
755  const Eigen::MatrixBase<Tangent_t> & v,
756  const Eigen::MatrixBase<Jacobian_t> & J_out) const
757  {
758  Jacobian_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t,J_out);
759  Eigen::Matrix<Scalar,6,6> Jtmp6;
760  Jexp6<SETTO>(MotionRef<const Tangent_t>(v.derived()), Jtmp6);
761 
762  Jout.template topRows<3>()
763  = Jtmp6.template topLeftCorner<3,3>() * Jout.template topRows<3>();
764  Jout.template topRows<3>().noalias()
765  += Jtmp6.template topRightCorner<3,3>() * Jout.template bottomRows<3>();
766  Jout.template bottomRows<3>()
767  = Jtmp6.template bottomRightCorner<3,3>() * Jout.template bottomRows<3>();
768  }
769 
770  template <class ConfigL_t, class ConfigR_t>
771  static Scalar squaredDistance_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
772  const Eigen::MatrixBase<ConfigR_t> & q1)
773  {
774  TangentVector_t t;
775  difference_impl(q0, q1, t);
776  return t.squaredNorm();
777  }
778 
779  template <class Config_t>
780  static void normalize_impl (const Eigen::MatrixBase<Config_t>& qout)
781  {
782  Config_t & qout_ = PINOCCHIO_EIGEN_CONST_CAST(Config_t,qout);
783  qout_.template tail<4>().normalize();
784  }
785 
786  template <class Config_t>
787  void random_impl (const Eigen::MatrixBase<Config_t>& qout) const
788  {
789  R3crossSO3_t().random(qout);
790  }
791 
792  template <class ConfigL_t, class ConfigR_t, class ConfigOut_t>
793  void randomConfiguration_impl(const Eigen::MatrixBase<ConfigL_t> & lower,
794  const Eigen::MatrixBase<ConfigR_t> & upper,
795  const Eigen::MatrixBase<ConfigOut_t> & qout)
796  const
797  {
798  R3crossSO3_t ().randomConfiguration(lower, upper, qout);
799  }
800 
801  template <class ConfigL_t, class ConfigR_t>
802  static bool isSameConfiguration_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
803  const Eigen::MatrixBase<ConfigR_t> & q1,
804  const Scalar & prec)
805  {
806  return R3crossSO3_t().isSameConfiguration(q0, q1, prec);
807  }
808  }; // struct SpecialEuclideanOperationTpl<3>
809 
810 } // namespace pinocchio
811 
812 #endif // ifndef __pinocchio_multibody_liegroup_special_euclidean_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::SpecialEuclideanOperationTpl< 2, _Scalar, _Options >::nv
static Index nv()
Get dimension of Lie Group tangent space.
Definition: special-euclidean.hpp:204
pinocchio::SpecialEuclideanOperationTpl< 3, _Scalar, _Options >::nv
static Index nv()
Get dimension of Lie Group tangent space.
Definition: special-euclidean.hpp:512
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::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::Jlog6
void Jlog6(const SE3Tpl< Scalar, Options > &M, const Eigen::MatrixBase< Matrix6Like > &Jlog)
Derivative of log6 where and .
Definition: explog.hpp:466
pinocchio::log6
MotionTpl< Scalar, Options > log6(const SE3Tpl< Scalar, Options > &M)
Log: SE3 -> se3.
Definition: explog.hpp:313
pinocchio::Jexp6
void Jexp6(const MotionDense< MotionDerived > &nu, const Eigen::MatrixBase< Matrix6Like > &Jexp)
Derivative of exp6 Computed as the inverse of Jlog6.
Definition: explog.hpp:349
pinocchio::isUnitary
bool isUnitary(const Eigen::MatrixBase< MatrixLike > &mat, const typename MatrixLike::RealScalar &prec=Eigen::NumTraits< typename MatrixLike::Scalar >::dummy_precision())
Check whether the input matrix is Unitary within the given precision.
Definition: matrix.hpp:100
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::forwardKinematics
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.
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::SpecialEuclideanOperationTpl< 3, _Scalar, _Options >::nq
static Index nq()
Definition: special-euclidean.hpp:507
pinocchio
Main pinocchio namespace.
Definition: treeview.dox:24
pinocchio::SpecialEuclideanOperationTpl< 3, _Scalar, _Options >::dDifference_impl
void dDifference_impl(const Eigen::MatrixBase< ConfigL_t > &q0, const Eigen::MatrixBase< ConfigR_t > &q1, const Eigen::MatrixBase< JacobianOut_t > &J) const
Definition: special-euclidean.hpp:545
pinocchio::SpecialEuclideanOperationTpl< 2, _Scalar, _Options >::nq
static Index nq()
Definition: special-euclidean.hpp:199
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::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
pinocchio::skew
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
pinocchio::normalize
void normalize(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorType > &qout)
Normalize a configuration vector.
Definition: joint-configuration.hpp:607
pinocchio::exp6
SE3Tpl< typename MotionDerived::Scalar, typename MotionDerived::Vector3 ::Options > exp6(const MotionDense< MotionDerived > &nu)
Exp: se3 -> SE3.
Definition: explog.hpp:236