pinocchio  2.6.3
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>
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 
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  PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix3Like, M, 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  static bool isNormalized_impl(const Eigen::MatrixBase<Config_t> & qin,
435  const Scalar& prec)
436  {
437  const Scalar norm = Scalar(qin.template tail<2>().norm());
438  using std::abs;
439  return abs(norm - Scalar(1.0)) < prec;
440  }
441 
442  template <class Config_t>
443  void random_impl(const Eigen::MatrixBase<Config_t> & qout) const
444  {
445  R2crossSO2_t().random(qout);
446  }
447 
448  template <class ConfigL_t, class ConfigR_t, class ConfigOut_t>
449  void randomConfiguration_impl(const Eigen::MatrixBase<ConfigL_t> & lower,
450  const Eigen::MatrixBase<ConfigR_t> & upper,
451  const Eigen::MatrixBase<ConfigOut_t> & qout)
452  const
453  {
454  R2crossSO2_t ().randomConfiguration(lower, upper, qout);
455  }
456 
457  template <class ConfigL_t, class ConfigR_t>
458  static bool isSameConfiguration_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
459  const Eigen::MatrixBase<ConfigR_t> & q1,
460  const Scalar & prec)
461  {
462  return R2crossSO2_t().isSameConfiguration(q0, q1, prec);
463  }
464 
465  protected:
466 
467  template<typename Matrix2Like, typename Vector2Like, typename Vector4Like>
468  static void forwardKinematics(const Eigen::MatrixBase<Matrix2Like> & R,
469  const Eigen::MatrixBase<Vector2Like> & t,
470  const Eigen::MatrixBase<Vector4Like> & q)
471  {
472  EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(Matrix2Like, Matrix2);
473  EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(Vector2Like, Vector2);
474  EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigVector_t,Vector4Like);
475 
476  PINOCCHIO_EIGEN_CONST_CAST(Vector2Like,t) = q.template head<2>();
477  const typename Vector4Like::Scalar & c_theta = q(2),
478  & s_theta = q(3);
479  PINOCCHIO_EIGEN_CONST_CAST(Matrix2Like,R) << c_theta, -s_theta, s_theta, c_theta;
480 
481  }
482  }; // struct SpecialEuclideanOperationTpl<2>
483 
484  template<typename _Scalar, int _Options>
485  struct traits< SpecialEuclideanOperationTpl<3,_Scalar,_Options> >
486  {
487  typedef _Scalar Scalar;
488  enum
489  {
490  Options = _Options,
491  NQ = 7,
492  NV = 6
493  };
494  };
495 
497  template<typename _Scalar, int _Options>
498  struct SpecialEuclideanOperationTpl<3,_Scalar,_Options>
499  : public LieGroupBase <SpecialEuclideanOperationTpl<3,_Scalar,_Options> >
500  {
501  PINOCCHIO_LIE_GROUP_TPL_PUBLIC_INTERFACE(SpecialEuclideanOperationTpl);
502 
504 
505  typedef Eigen::Quaternion<Scalar,Options> Quaternion_t;
506  typedef Eigen::Map< Quaternion_t> QuaternionMap_t;
507  typedef Eigen::Map<const Quaternion_t> ConstQuaternionMap_t;
509  typedef SE3Tpl<Scalar,Options> SE3;
510  typedef typename Eigen::NumTraits<Scalar>::Real RealScalar;
511 
516  static Index nq()
517  {
518  return NQ;
519  }
521  static Index nv()
522  {
523  return NV;
524  }
525 
526  static ConfigVector_t neutral()
527  {
528  ConfigVector_t n; n.template head<6>().setZero(); n[6] = 1;
529  return n;
530  }
531 
532  static std::string name()
533  {
534  return std::string("SE(3)");
535  }
536 
537  template <class ConfigL_t, class ConfigR_t, class Tangent_t>
538  static void difference_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
539  const Eigen::MatrixBase<ConfigR_t> & q1,
540  const Eigen::MatrixBase<Tangent_t> & d)
541  {
542  ConstQuaternionMap_t quat0 (q0.derived().template tail<4>().data());
543  assert(quaternion::isNormalized(quat0,RealScalar(PINOCCHIO_DEFAULT_QUATERNION_NORM_TOLERANCE_VALUE)));
544  ConstQuaternionMap_t quat1 (q1.derived().template tail<4>().data());
545  assert(quaternion::isNormalized(quat1,RealScalar(PINOCCHIO_DEFAULT_QUATERNION_NORM_TOLERANCE_VALUE)));
546 
547  PINOCCHIO_EIGEN_CONST_CAST(Tangent_t,d)
548  = log6( SE3(quat0.matrix(), q0.derived().template head<3>()).inverse()
549  * SE3(quat1.matrix(), q1.derived().template head<3>())).toVector();
550  }
551 
553  template <ArgumentPosition arg, class ConfigL_t, class ConfigR_t, class JacobianOut_t>
554  void dDifference_impl (const Eigen::MatrixBase<ConfigL_t> & q0,
555  const Eigen::MatrixBase<ConfigR_t> & q1,
556  const Eigen::MatrixBase<JacobianOut_t> & J) const
557  {
558  typedef typename SE3::Vector3 Vector3;
559  typedef typename SE3::Matrix3 Matrix3;
560 
561  ConstQuaternionMap_t quat0 (q0.derived().template tail<4>().data());
562  assert(quaternion::isNormalized(quat0,RealScalar(PINOCCHIO_DEFAULT_QUATERNION_NORM_TOLERANCE_VALUE)));
563  ConstQuaternionMap_t quat1 (q1.derived().template tail<4>().data());
564  assert(quaternion::isNormalized(quat1,RealScalar(PINOCCHIO_DEFAULT_QUATERNION_NORM_TOLERANCE_VALUE)));
565 
566  Matrix3 R0(quat0.matrix()), R1 (quat1.matrix());
567  assert(isUnitary(R0)); assert(isUnitary(R1));
568 
569  const SE3 M ( SE3(R0, q0.template head<3>()).inverse()
570  * SE3(R1, q1.template head<3>()));
571 
572  if (arg == ARG0) {
573  JacobianMatrix_t J1;
574  Jlog6 (M, J1);
575 
576  const Vector3 p1_p0 = R1.transpose()*(q1.template head<3>() - q0.template head<3>());
577 
578  JacobianOut_t & J0 = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J);
579  J0.template bottomRightCorner<3,3> ().noalias() = J0.template topLeftCorner <3,3> ().noalias() = - M.rotation().transpose();
580  J0.template topRightCorner<3,3> ().noalias() = skew(p1_p0) * M.rotation().transpose(); // = R1.T * skew(q1_t - q0_t) * R0;
581  J0.template bottomLeftCorner<3,3> ().setZero();
582  J0.applyOnTheLeft(J1);
583  }
584  else if (arg == ARG1) {
585  Jlog6 (M, J);
586  }
587  }
588 
589  template <class ConfigIn_t, class Velocity_t, class ConfigOut_t>
590  static void integrate_impl(const Eigen::MatrixBase<ConfigIn_t> & q,
591  const Eigen::MatrixBase<Velocity_t> & v,
592  const Eigen::MatrixBase<ConfigOut_t> & qout)
593  {
594  ConfigOut_t & out = PINOCCHIO_EIGEN_CONST_CAST(ConfigOut_t,qout);
595  ConstQuaternionMap_t quat(q.derived().template tail<4>().data());
596  assert(quaternion::isNormalized(quat,RealScalar(PINOCCHIO_DEFAULT_QUATERNION_NORM_TOLERANCE_VALUE)));
597  QuaternionMap_t res_quat (out.template tail<4>().data());
598 
599  using internal::if_then_else;
600 
601  SE3 M0 (quat.matrix(), q.derived().template head<3>());
602  MotionRef<const Velocity_t> mref_v(v.derived());
603  SE3 M1 (M0 * exp6(mref_v));
604 
605  out.template head<3>() = M1.translation();
606  quaternion::assignQuaternion(res_quat,M1.rotation()); // required by CasADi
607  const Scalar dot_product = res_quat.dot(quat);
608  for(Eigen::DenseIndex k = 0; k < 4; ++k)
609  {
610  res_quat.coeffs().coeffRef(k) = if_then_else(internal::LT, dot_product, Scalar(0),
611  -res_quat.coeffs().coeff(k),
612  res_quat.coeffs().coeff(k));
613  }
614 
615  // Norm of qs might be epsilon-different to 1, so M1.rotation might be epsilon-different to a rotation matrix.
616  // It is then safer to re-normalized after converting M1.rotation to quaternion.
618  assert(quaternion::isNormalized(res_quat,RealScalar(PINOCCHIO_DEFAULT_QUATERNION_NORM_TOLERANCE_VALUE)));
619  }
620 
621  template <class Config_t, class Jacobian_t>
622  static void integrateCoeffWiseJacobian_impl(const Eigen::MatrixBase<Config_t> & q,
623  const Eigen::MatrixBase<Jacobian_t> & J)
624  {
625  assert(J.rows() == nq() && J.cols() == nv() && "J is not of the right dimension");
626 
627  typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(Config_t) ConfigPlainType;
628  typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(Jacobian_t) JacobianPlainType;
629  typedef typename ConfigPlainType::Scalar Scalar;
630 
631  Jacobian_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t,J);
632  Jout.setZero();
633 
634  ConstQuaternionMap_t quat_map(q.derived().template tail<4>().data());
635  assert(quaternion::isNormalized(quat_map,RealScalar(PINOCCHIO_DEFAULT_QUATERNION_NORM_TOLERANCE_VALUE)));
636  Jout.template topLeftCorner<3,3>() = quat_map.toRotationMatrix();
637 // Jexp3(quat,Jout.template bottomRightCorner<4,3>());
638 
639  typedef Eigen::Matrix<Scalar,4,3,JacobianPlainType::Options|Eigen::RowMajor> Jacobian43;
641  Jacobian43 Jexp3QuatCoeffWise;
642 
643  Scalar theta;
644  typename SE3::Vector3 v = quaternion::log3(quat_map,theta);
645  quaternion::Jexp3CoeffWise(v,Jexp3QuatCoeffWise);
646  typename SE3::Matrix3 Jlog;
647  Jlog3(theta,v,Jlog);
648 
649 // std::cout << "Jexp3QuatCoeffWise\n" << Jexp3QuatCoeffWise << std::endl;
650 // std::cout << "Jlog\n" << Jlog << std::endl;
651 
652 // if(quat_map.w() >= 0.) // comes from the log3 for quaternions which may change the sign.
653  if(quat_map.coeffs()[3] >= Scalar(0)) // comes from the log3 for quaternions which may change the sign.
654  PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t,J).template bottomRightCorner<4,3>().noalias() = Jexp3QuatCoeffWise * Jlog;
655  else
656  PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t,J).template bottomRightCorner<4,3>().noalias() = -Jexp3QuatCoeffWise * Jlog;
657  }
658 
659  template <class Config_t, class Tangent_t, class JacobianOut_t>
660  static void dIntegrate_dq_impl(const Eigen::MatrixBase<Config_t > & /*q*/,
661  const Eigen::MatrixBase<Tangent_t> & v,
662  const Eigen::MatrixBase<JacobianOut_t>& J,
663  const AssignmentOperatorType op=SETTO)
664  {
665  JacobianOut_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J);
666 
667  switch(op)
668  {
669  case SETTO:
670  Jout = exp6(MotionRef<const Tangent_t>(v.derived())).toDualActionMatrix().transpose();
671  break;
672  case ADDTO:
673  Jout += exp6(MotionRef<const Tangent_t>(v.derived())).toDualActionMatrix().transpose();
674  break;
675  case RMTO:
676  Jout -= exp6(MotionRef<const Tangent_t>(v.derived())).toDualActionMatrix().transpose();
677  break;
678  default:
679  assert(false && "Wrong Op requesed value");
680  break;
681  }
682  }
683 
684  template <class Config_t, class Tangent_t, class JacobianOut_t>
685  static void dIntegrate_dv_impl(const Eigen::MatrixBase<Config_t > & /*q*/,
686  const Eigen::MatrixBase<Tangent_t> & v,
687  const Eigen::MatrixBase<JacobianOut_t>& J,
688  const AssignmentOperatorType op=SETTO)
689  {
690  switch(op)
691  {
692  case SETTO:
693  Jexp6<SETTO>(MotionRef<const Tangent_t>(v.derived()), J.derived());
694  break;
695  case ADDTO:
696  Jexp6<ADDTO>(MotionRef<const Tangent_t>(v.derived()), J.derived());
697  break;
698  case RMTO:
699  Jexp6<RMTO>(MotionRef<const Tangent_t>(v.derived()), J.derived());
700  break;
701  default:
702  assert(false && "Wrong Op requesed value");
703  break;
704  }
705  }
706 
707  template <class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
708  void dIntegrateTransport_dq_impl(const Eigen::MatrixBase<Config_t > & /*q*/,
709  const Eigen::MatrixBase<Tangent_t> & v,
710  const Eigen::MatrixBase<JacobianIn_t> & Jin,
711  const Eigen::MatrixBase<JacobianOut_t> & J_out) const
712  {
713  JacobianOut_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J_out);
714  Eigen::Matrix<Scalar,6,6> Jtmp6;
715  Jtmp6 = exp6(MotionRef<const Tangent_t>(v.derived())).toDualActionMatrix().transpose();
716 
717  Jout.template topRows<3>().noalias()
718  = Jtmp6.template topLeftCorner<3,3>() * Jin.template topRows<3>();
719  Jout.template topRows<3>().noalias()
720  += Jtmp6.template topRightCorner<3,3>() * Jin.template bottomRows<3>();
721  Jout.template bottomRows<3>().noalias()
722  = Jtmp6.template bottomRightCorner<3,3>() * Jin.template bottomRows<3>();
723  }
724 
725  template <class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
726  void dIntegrateTransport_dv_impl(const Eigen::MatrixBase<Config_t > & /*q*/,
727  const Eigen::MatrixBase<Tangent_t> & v,
728  const Eigen::MatrixBase<JacobianIn_t> & Jin,
729  const Eigen::MatrixBase<JacobianOut_t> & J_out) const
730  {
731  JacobianOut_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J_out);
732  Eigen::Matrix<Scalar,6,6> Jtmp6;
733  Jexp6<SETTO>(MotionRef<const Tangent_t>(v.derived()), Jtmp6);
734 
735  Jout.template topRows<3>().noalias()
736  = Jtmp6.template topLeftCorner<3,3>() * Jin.template topRows<3>();
737  Jout.template topRows<3>().noalias()
738  += Jtmp6.template topRightCorner<3,3>() * Jin.template bottomRows<3>();
739  Jout.template bottomRows<3>().noalias()
740  = Jtmp6.template bottomRightCorner<3,3>() * Jin.template bottomRows<3>();
741  }
742 
743 
744  template <class Config_t, class Tangent_t, class Jacobian_t>
745  void dIntegrateTransport_dq_impl(const Eigen::MatrixBase<Config_t > & /*q*/,
746  const Eigen::MatrixBase<Tangent_t> & v,
747  const Eigen::MatrixBase<Jacobian_t> & J_out) const
748  {
749  Jacobian_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t,J_out);
750  Eigen::Matrix<Scalar,6,6> Jtmp6;
751  Jtmp6 = exp6(MotionRef<const Tangent_t>(v.derived())).toDualActionMatrix().transpose();
752 
753  //Aliasing
754  Jout.template topRows<3>()
755  = Jtmp6.template topLeftCorner<3,3>() * Jout.template topRows<3>();
756  Jout.template topRows<3>().noalias()
757  += Jtmp6.template topRightCorner<3,3>() * Jout.template bottomRows<3>();
758  Jout.template bottomRows<3>()
759  = Jtmp6.template bottomRightCorner<3,3>() * Jout.template bottomRows<3>();
760  }
761 
762  template <class Config_t, class Tangent_t, class Jacobian_t>
763  void dIntegrateTransport_dv_impl(const Eigen::MatrixBase<Config_t > & /*q*/,
764  const Eigen::MatrixBase<Tangent_t> & v,
765  const Eigen::MatrixBase<Jacobian_t> & J_out) const
766  {
767  Jacobian_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t,J_out);
768  Eigen::Matrix<Scalar,6,6> Jtmp6;
769  Jexp6<SETTO>(MotionRef<const Tangent_t>(v.derived()), Jtmp6);
770 
771  Jout.template topRows<3>()
772  = Jtmp6.template topLeftCorner<3,3>() * Jout.template topRows<3>();
773  Jout.template topRows<3>().noalias()
774  += Jtmp6.template topRightCorner<3,3>() * Jout.template bottomRows<3>();
775  Jout.template bottomRows<3>()
776  = Jtmp6.template bottomRightCorner<3,3>() * Jout.template bottomRows<3>();
777  }
778 
779  template <class ConfigL_t, class ConfigR_t>
780  static Scalar squaredDistance_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
781  const Eigen::MatrixBase<ConfigR_t> & q1)
782  {
783  TangentVector_t t;
784  difference_impl(q0, q1, t);
785  return t.squaredNorm();
786  }
787 
788  template <class Config_t>
789  static void normalize_impl (const Eigen::MatrixBase<Config_t>& qout)
790  {
791  Config_t & qout_ = PINOCCHIO_EIGEN_CONST_CAST(Config_t,qout);
792  qout_.template tail<4>().normalize();
793  }
794 
795  template <class Config_t>
796  static bool isNormalized_impl(const Eigen::MatrixBase<Config_t>& qin,
797  const Scalar& prec)
798  {
799  Scalar norm = Scalar(qin.template tail<4>().norm());
800  using std::abs;
801  return abs(norm - Scalar(1.0)) < prec;
802  }
803 
804  template <class Config_t>
805  void random_impl (const Eigen::MatrixBase<Config_t>& qout) const
806  {
807  R3crossSO3_t().random(qout);
808  }
809 
810  template <class ConfigL_t, class ConfigR_t, class ConfigOut_t>
811  void randomConfiguration_impl(const Eigen::MatrixBase<ConfigL_t> & lower,
812  const Eigen::MatrixBase<ConfigR_t> & upper,
813  const Eigen::MatrixBase<ConfigOut_t> & qout)
814  const
815  {
816  R3crossSO3_t ().randomConfiguration(lower, upper, qout);
817  }
818 
819  template <class ConfigL_t, class ConfigR_t>
820  static bool isSameConfiguration_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
821  const Eigen::MatrixBase<ConfigR_t> & q1,
822  const Scalar & prec)
823  {
824  return R3crossSO3_t().isSameConfiguration(q0, q1, prec);
825  }
826  }; // struct SpecialEuclideanOperationTpl<3>
827 
828 } // namespace pinocchio
829 
830 #endif // ifndef __pinocchio_multibody_liegroup_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:193
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:88
void Jlog6(const SE3Tpl< Scalar, Options > &M, const Eigen::MatrixBase< Matrix6Like > &Jlog)
Derivative of log6 where and .
Definition: explog.hpp:538
MotionTpl< Scalar, Options > log6(const SE3Tpl< Scalar, Options > &M)
Log: SE3 -> se3.
Definition: explog.hpp:373
void Jexp6(const MotionDense< MotionDerived > &nu, const Eigen::MatrixBase< Matrix6Like > &Jexp)
Derivative of exp6 Computed as the inverse of Jlog6.
Definition: explog.hpp:409
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:140
void Jexp3CoeffWise(const Eigen::MatrixBase< Vector3Like > &v, const Eigen::MatrixBase< Matrix43Like > &Jexp)
Derivative of where is a small perturbation of at identity.
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 dDifference_impl(const Eigen::MatrixBase< ConfigL_t > &q0, const Eigen::MatrixBase< ConfigR_t > &q1, const Eigen::MatrixBase< JacobianOut_t > &J) const
void neutral(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ReturnType > &qout)
Return the neutral configuration element related to the model configuration space.
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
Common traits structure to fully define base classes for CRTP.
Definition: fwd.hpp:44
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:296