special-euclidean.hpp
Go to the documentation of this file.
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  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 
503  typedef CartesianProductOperation <VectorSpaceOperationTpl<3,Scalar,Options>, SpecialOrthogonalOperationTpl<3,Scalar,Options> > R3crossSO3_t;
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;
508  typedef SE3Tpl<Scalar,Options> Transformation_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  Quaternion_t const quat(q.derived().template tail<4>());
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.
617  quaternion::firstOrderNormalize(res_quat);
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;
640  typedef SE3Tpl<Scalar,ConfigPlainType::Options> SE3;
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__
pinocchio::SpecialEuclideanOperationTpl< 2, _Scalar, _Options >::R2_t
VectorSpaceOperationTpl< 2, Scalar, Options > R2_t
Definition: special-euclidean.hpp:50
pinocchio::v
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > const Eigen::MatrixBase< TangentVectorType > & v
Definition: joint-configuration.hpp:746
pinocchio::SpecialEuclideanOperationTpl< 3, _Scalar, _Options >::dIntegrateTransport_dv_impl
void dIntegrateTransport_dv_impl(const Eigen::MatrixBase< Config_t > &, const Eigen::MatrixBase< Tangent_t > &v, const Eigen::MatrixBase< JacobianIn_t > &Jin, const Eigen::MatrixBase< JacobianOut_t > &J_out) const
Definition: special-euclidean.hpp:726
pinocchio::SpecialEuclideanOperationTpl< 2, _Scalar, _Options >::exp
static void exp(const Eigen::MatrixBase< TangentVector > &v, const Eigen::MatrixBase< Matrix2Like > &R, const Eigen::MatrixBase< Vector2Like > &t)
Definition: special-euclidean.hpp:58
V
V
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< 3, _Scalar, _Options >::integrate_impl
static void integrate_impl(const Eigen::MatrixBase< ConfigIn_t > &q, const Eigen::MatrixBase< Velocity_t > &v, const Eigen::MatrixBase< ConfigOut_t > &qout)
Definition: special-euclidean.hpp:590
pinocchio::SpecialEuclideanOperationTpl< 3, _Scalar, _Options >::isSameConfiguration_impl
static bool isSameConfiguration_impl(const Eigen::MatrixBase< ConfigL_t > &q0, const Eigen::MatrixBase< ConfigR_t > &q1, const Scalar &prec)
Definition: special-euclidean.hpp:820
pinocchio::SpecialEuclideanOperationTpl< 2, _Scalar, _Options >::nq
static Index nq()
Definition: special-euclidean.hpp:199
pinocchio::SpecialEuclideanOperationTpl< 2, _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:235
dpendulum.NQ
int NQ
Definition: dpendulum.py:8
pinocchio::SpecialEuclideanOperationTpl< 2, _Scalar, _Options >::integrateCoeffWiseJacobian_impl
static void integrateCoeffWiseJacobian_impl(const Eigen::MatrixBase< Config_t > &q, const Eigen::MatrixBase< Jacobian_t > &J)
Definition: special-euclidean.hpp:280
PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE
#define PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE(type, M, nrows, ncols)
Ensure that a matrix (or vector) is of correct size (compile-time and run-time assertion) ...
Definition: src/macros.hpp:54
n
Vec3f n
pinocchio::SpecialEuclideanOperationTpl< 2, _Scalar, _Options >::toInverseActionMatrix
static void toInverseActionMatrix(const Eigen::MatrixBase< Matrix2Like > &R, const Eigen::MatrixBase< Vector2Like > &t, const Eigen::MatrixBase< Matrix3Like > &M, const AssignmentOperatorType op)
Definition: special-euclidean.hpp:89
pinocchio::SpecialEuclideanOperationTpl< 3, _Scalar, _Options >::integrateCoeffWiseJacobian_impl
static void integrateCoeffWiseJacobian_impl(const Eigen::MatrixBase< Config_t > &q, const Eigen::MatrixBase< Jacobian_t > &J)
Definition: special-euclidean.hpp:622
pinocchio::SpecialEuclideanOperationTpl< 2, _Scalar, _Options >::isSameConfiguration_impl
static bool isSameConfiguration_impl(const Eigen::MatrixBase< ConfigL_t > &q0, const Eigen::MatrixBase< ConfigR_t > &q1, const Scalar &prec)
Definition: special-euclidean.hpp:458
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::SpecialEuclideanOperationTpl< 2, _Scalar, _Options >::dIntegrate_dv_impl
static void dIntegrate_dv_impl(const Eigen::MatrixBase< Config_t > &, const Eigen::MatrixBase< Tangent_t > &v, const Eigen::MatrixBase< JacobianOut_t > &J, const AssignmentOperatorType op=SETTO)
Definition: special-euclidean.hpp:311
pinocchio::Jlog3
void Jlog3(const Scalar &theta, const Eigen::MatrixBase< Vector3Like > &log, const Eigen::MatrixBase< Matrix3Like > &Jlog)
Derivative of log3.
Definition: src/spatial/explog.hpp:223
pinocchio::SpecialEuclideanOperationTpl< 3, _Scalar, _Options >::dIntegrateTransport_dv_impl
void dIntegrateTransport_dv_impl(const Eigen::MatrixBase< Config_t > &, const Eigen::MatrixBase< Tangent_t > &v, const Eigen::MatrixBase< Jacobian_t > &J_out) const
Definition: special-euclidean.hpp:763
pinocchio::SpecialEuclideanOperationTpl< 2, _Scalar, _Options >::Vector2
Eigen::Matrix< Scalar, 2, 1, Options > Vector2
Definition: special-euclidean.hpp:55
pinocchio::SpecialEuclideanOperationTpl< 3, _Scalar, _Options >::ConstQuaternionMap_t
Eigen::Map< const Quaternion_t > ConstQuaternionMap_t
Definition: special-euclidean.hpp:507
pinocchio::SpecialEuclideanOperationTpl< 2, _Scalar, _Options >::name
static std::string name()
Definition: special-euclidean.hpp:215
pinocchio::quaternion::firstOrderNormalize
void firstOrderNormalize(const Eigen::QuaternionBase< D > &q)
Definition: math/quaternion.hpp:88
pinocchio::SpecialEuclideanOperationTpl< 3, _Scalar, _Options >::normalize_impl
static void normalize_impl(const Eigen::MatrixBase< Config_t > &qout)
Definition: special-euclidean.hpp:789
pinocchio::SpecialEuclideanOperationTpl< 3, _Scalar, _Options >::dIntegrate_dv_impl
static void dIntegrate_dv_impl(const Eigen::MatrixBase< Config_t > &, const Eigen::MatrixBase< Tangent_t > &v, const Eigen::MatrixBase< JacobianOut_t > &J, const AssignmentOperatorType op=SETTO)
Definition: special-euclidean.hpp:685
pinocchio::SpecialEuclideanOperationTpl< 2, _Scalar, _Options >::randomConfiguration_impl
void randomConfiguration_impl(const Eigen::MatrixBase< ConfigL_t > &lower, const Eigen::MatrixBase< ConfigR_t > &upper, const Eigen::MatrixBase< ConfigOut_t > &qout) const
Definition: special-euclidean.hpp:449
pinocchio::Jlog6
void Jlog6(const SE3Tpl< Scalar, Options > &M, const Eigen::MatrixBase< Matrix6Like > &Jlog)
Derivative of log6.
Definition: src/spatial/explog.hpp:594
pinocchio::log6
MotionTpl< Scalar, Options > log6(const SE3Tpl< Scalar, Options > &M)
Log: SE3 -> se3.
Definition: src/spatial/explog.hpp:403
PINOCCHIO_LIE_GROUP_TPL_PUBLIC_INTERFACE
#define PINOCCHIO_LIE_GROUP_TPL_PUBLIC_INTERFACE(Derived)
Definition: liegroup-base.hpp:36
pinocchio::SpecialEuclideanOperationTpl< 2, _Scalar, _Options >::Matrix2
Eigen::Matrix< Scalar, 2, 2, Options > Matrix2
Definition: special-euclidean.hpp:54
pinocchio::Jexp6
void Jexp6(const MotionDense< MotionDerived > &nu, const Eigen::MatrixBase< Matrix6Like > &Jexp)
Derivative of exp6 Computed as the inverse of Jlog6.
Definition: src/spatial/explog.hpp:439
pinocchio::SpecialEuclideanOperationTpl< 2, _Scalar, _Options >::dIntegrateTransport_dv_impl
void dIntegrateTransport_dv_impl(const Eigen::MatrixBase< Config_t > &, const Eigen::MatrixBase< Tangent_t > &v, const Eigen::MatrixBase< Jacobian_t > &J) const
Definition: special-euclidean.hpp:407
pinocchio::q
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > & q
Definition: joint-configuration.hpp:746
PINOCCHIO_EIGEN_CONST_CAST
#define PINOCCHIO_EIGEN_CONST_CAST(TYPE, OBJ)
Macro for an automatic const_cast.
Definition: eigen-macros.hpp:43
pinocchio::SpecialEuclideanOperationTpl< 2, _Scalar, _Options >::normalize_impl
static void normalize_impl(const Eigen::MatrixBase< Config_t > &qout)
Definition: special-euclidean.hpp:428
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: math/matrix.hpp:140
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:157
pinocchio::SpecialEuclideanOperationTpl< 2, _Scalar, _Options >::neutral
static ConfigVector_t neutral()
Definition: special-euclidean.hpp:209
pinocchio::python::Scalar
SE3::Scalar Scalar
Definition: conversions.cpp:13
pinocchio::SpecialEuclideanOperationTpl< 3, _Scalar, _Options >::dIntegrateTransport_dq_impl
void dIntegrateTransport_dq_impl(const Eigen::MatrixBase< Config_t > &, const Eigen::MatrixBase< Tangent_t > &v, const Eigen::MatrixBase< Jacobian_t > &J_out) const
Definition: special-euclidean.hpp:745
pinocchio::SpecialEuclideanOperationTpl< 3, _Scalar, _Options >::nv
static Index nv()
Get dimension of Lie Group tangent space.
Definition: special-euclidean.hpp:521
pinocchio::SpecialEuclideanOperationTpl< 2, _Scalar, _Options >::SO2_t
SpecialOrthogonalOperationTpl< 2, Scalar, Options > SO2_t
Definition: special-euclidean.hpp:51
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::AssignmentOperatorType
AssignmentOperatorType
Definition: src/fwd.hpp:68
pinocchio::SpecialEuclideanOperationTpl< 2, _Scalar, _Options >::dIntegrateTransport_dq_impl
void dIntegrateTransport_dq_impl(const Eigen::MatrixBase< Config_t > &, const Eigen::MatrixBase< Tangent_t > &v, const Eigen::MatrixBase< JacobianIn_t > &Jin, const Eigen::MatrixBase< JacobianOut_t > &J_out) const
Definition: special-euclidean.hpp:347
pinocchio::quaternion::log3
Eigen::Matrix< typename QuaternionLike::Scalar, 3, 1, PINOCCHIO_EIGEN_PLAIN_TYPE(typename QuaternionLike::Vector3)::Options > log3(const Eigen::QuaternionBase< QuaternionLike > &quat, typename QuaternionLike::Scalar &theta)
Same as log3 but with a unit quaternion as input.
Definition: explog-quaternion.hpp:84
pinocchio::SpecialEuclideanOperationTpl< 2, _Scalar, _Options >::random_impl
void random_impl(const Eigen::MatrixBase< Config_t > &qout) const
Definition: special-euclidean.hpp:443
pinocchio::SE3Tpl::inverse
SE3Tpl inverse() const
aXb = bXa.inverse()
Definition: spatial/se3-tpl.hpp:115
pinocchio::SpecialEuclideanOperationTpl< 3, _Scalar, _Options >::RealScalar
Eigen::NumTraits< Scalar >::Real RealScalar
Definition: special-euclidean.hpp:510
pinocchio::SpecialEuclideanOperationTpl< 3, _Scalar, _Options >::nq
static Index nq()
Definition: special-euclidean.hpp:516
pinocchio::SpecialEuclideanOperationTpl< 2, _Scalar, _Options >::nv
static Index nv()
Get dimension of Lie Group tangent space.
Definition: special-euclidean.hpp:204
pinocchio::traits< SpecialEuclideanOperationTpl< 3, _Scalar, _Options > >::Scalar
_Scalar Scalar
Definition: special-euclidean.hpp:487
pinocchio::traits< SpecialEuclideanOperationTpl< 2, _Scalar, _Options > >::Scalar
_Scalar Scalar
Definition: special-euclidean.hpp:34
pinocchio::q1
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorIn1 > const Eigen::MatrixBase< ConfigVectorIn2 > & q1
Definition: joint-configuration.hpp:784
R
R
quat
quat
d
FCL_REAL d
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< 2, _Scalar, _Options >::isNormalized_impl
static bool isNormalized_impl(const Eigen::MatrixBase< Config_t > &qin, const Scalar &prec)
Definition: special-euclidean.hpp:434
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:554
pinocchio::SpecialEuclideanOperationTpl< 3, _Scalar, _Options >::squaredDistance_impl
static Scalar squaredDistance_impl(const Eigen::MatrixBase< ConfigL_t > &q0, const Eigen::MatrixBase< ConfigR_t > &q1)
Definition: special-euclidean.hpp:780
pinocchio::SE3Tpl< Scalar, Options >::Vector3
traits< SE3Tpl >::Vector3 Vector3
Definition: spatial/se3-tpl.hpp:53
pinocchio::SpecialEuclideanOperationTpl< 3, _Scalar, _Options >::dIntegrateTransport_dq_impl
void dIntegrateTransport_dq_impl(const Eigen::MatrixBase< Config_t > &, const Eigen::MatrixBase< Tangent_t > &v, const Eigen::MatrixBase< JacobianIn_t > &Jin, const Eigen::MatrixBase< JacobianOut_t > &J_out) const
Definition: special-euclidean.hpp:708
pinocchio::SpecialEuclideanOperationTpl< 3, _Scalar, _Options >::Transformation_t
SE3Tpl< Scalar, Options > Transformation_t
Definition: special-euclidean.hpp:508
pinocchio
Main pinocchio namespace.
Definition: timings.cpp:28
pinocchio::SpecialEuclideanOperationTpl< 2, _Scalar, _Options >::dIntegrateTransport_dv_impl
void dIntegrateTransport_dv_impl(const Eigen::MatrixBase< Config_t > &, const Eigen::MatrixBase< Tangent_t > &v, const Eigen::MatrixBase< JacobianIn_t > &Jin, const Eigen::MatrixBase< JacobianOut_t > &J_out) const
Definition: special-euclidean.hpp:366
pinocchio::SpecialEuclideanOperationTpl< 3, _Scalar, _Options >::random_impl
void random_impl(const Eigen::MatrixBase< Config_t > &qout) const
Definition: special-euclidean.hpp:805
pinocchio::q0
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorIn1 > & q0
Definition: joint-configuration.hpp:784
pinocchio::quaternion::assignQuaternion
void assignQuaternion(Eigen::QuaternionBase< D > &quat, const Eigen::MatrixBase< Matrix3 > &R)
Definition: math/quaternion.hpp:209
pinocchio::SpecialEuclideanOperationTpl< 3, _Scalar, _Options >::neutral
static ConfigVector_t neutral()
Definition: special-euclidean.hpp:526
pinocchio::PINOCCHIO_EIGEN_PLAIN_TYPE
PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorType) integrate(const ModelTpl< Scalar
Integrate a configuration vector for the specified model for a tangent vector during one unit time...
pinocchio::SpecialEuclideanOperationTpl< 3, _Scalar, _Options >::randomConfiguration_impl
void randomConfiguration_impl(const Eigen::MatrixBase< ConfigL_t > &lower, const Eigen::MatrixBase< ConfigR_t > &upper, const Eigen::MatrixBase< ConfigOut_t > &qout) const
Definition: special-euclidean.hpp:811
pinocchio::SpecialEuclideanOperationTpl< 3, _Scalar, _Options >::QuaternionMap_t
Eigen::Map< Quaternion_t > QuaternionMap_t
Definition: special-euclidean.hpp:506
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: math/quaternion.hpp:225
PINOCCHIO_DEFAULT_QUATERNION_NORM_TOLERANCE_VALUE
#define PINOCCHIO_DEFAULT_QUATERNION_NORM_TOLERANCE_VALUE
Definition: math/quaternion.hpp:9
pinocchio::SpecialEuclideanOperationTpl< 3, _Scalar, _Options >::dIntegrate_dq_impl
static void dIntegrate_dq_impl(const Eigen::MatrixBase< Config_t > &, const Eigen::MatrixBase< Tangent_t > &v, const Eigen::MatrixBase< JacobianOut_t > &J, const AssignmentOperatorType op=SETTO)
Definition: special-euclidean.hpp:660
pinocchio::SpecialEuclideanOperationTpl< 2, _Scalar, _Options >::R2crossSO2_t
CartesianProductOperation< R2_t, SO2_t > R2crossSO2_t
Definition: special-euclidean.hpp:52
pinocchio::SpecialEuclideanOperationTpl< 2, _Scalar, _Options >::Jlog
static void Jlog(const Eigen::MatrixBase< Matrix2Like > &R, const Eigen::MatrixBase< Vector2Like > &p, const Eigen::MatrixBase< JacobianOutLike > &J)
Definition: special-euclidean.hpp:158
pinocchio::SpecialEuclideanOperationTpl< 2, _Scalar, _Options >::difference_impl
static void difference_impl(const Eigen::MatrixBase< ConfigL_t > &q0, const Eigen::MatrixBase< ConfigR_t > &q1, const Eigen::MatrixBase< Tangent_t > &d)
Definition: special-euclidean.hpp:221
pinocchio::SpecialEuclideanOperationTpl< 3, _Scalar, _Options >::difference_impl
static void difference_impl(const Eigen::MatrixBase< ConfigL_t > &q0, const Eigen::MatrixBase< ConfigR_t > &q1, const Eigen::MatrixBase< Tangent_t > &d)
Definition: special-euclidean.hpp:538
pinocchio::SpecialEuclideanOperationTpl< 2, _Scalar, _Options >::forwardKinematics
static void forwardKinematics(const Eigen::MatrixBase< Matrix2Like > &R, const Eigen::MatrixBase< Vector2Like > &t, const Eigen::MatrixBase< Vector4Like > &q)
Definition: special-euclidean.hpp:468
setZero
void setZero(std::vector< MatType, Eigen::aligned_allocator< MatType > > &Ms)
pinocchio::traits
Common traits structure to fully define base classes for CRTP.
Definition: src/fwd.hpp:44
pinocchio::internal::if_then_else
if_then_else_impl< LhsType, RhsType, ThenType, ElseType >::ReturnType if_then_else(const ComparisonOperators op, const LhsType &lhs_value, const RhsType &rhs_value, const ThenType &then_value, const ElseType &else_value)
Definition: utils/static-if.hpp:77
t
Transform3f t
pinocchio::SpecialEuclideanOperationTpl< 3, _Scalar, _Options >::name
static std::string name()
Definition: special-euclidean.hpp:532
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:609
pinocchio::SpecialEuclideanOperationTpl< 3, _Scalar, _Options >::SE3
SE3Tpl< Scalar, Options > SE3
Definition: special-euclidean.hpp:509
pinocchio::SpecialEuclideanOperationTpl< 3, _Scalar, _Options >::R3crossSO3_t
CartesianProductOperation< VectorSpaceOperationTpl< 3, Scalar, Options >, SpecialOrthogonalOperationTpl< 3, Scalar, Options > > R3crossSO3_t
Definition: special-euclidean.hpp:503
pinocchio::SpecialEuclideanOperationTpl< 3, _Scalar, _Options >::isNormalized_impl
static bool isNormalized_impl(const Eigen::MatrixBase< Config_t > &qin, const Scalar &prec)
Definition: special-euclidean.hpp:796
pinocchio::exp6
SE3Tpl< typename MotionDerived::Scalar, PINOCCHIO_EIGEN_PLAIN_TYPE(typename MotionDerived::Vector3)::Options > exp6(const MotionDense< MotionDerived > &nu)
Exp: se3 -> SE3.
Definition: src/spatial/explog.hpp:326
M
M
pinocchio::SE3Tpl< Scalar, Options >::Matrix3
traits< SE3Tpl >::Matrix3 Matrix3
Definition: spatial/se3-tpl.hpp:54
dcrba.NV
NV
Definition: dcrba.py:444
pinocchio::SpecialEuclideanOperationTpl< 2, _Scalar, _Options >::integrate_impl
static void integrate_impl(const Eigen::MatrixBase< ConfigIn_t > &q, const Eigen::MatrixBase< Velocity_t > &v, const Eigen::MatrixBase< ConfigOut_t > &qout)
Definition: special-euclidean.hpp:264
pinocchio::SpecialEuclideanOperationTpl< 2, _Scalar, _Options >::dIntegrateTransport_dq_impl
void dIntegrateTransport_dq_impl(const Eigen::MatrixBase< Config_t > &, const Eigen::MatrixBase< Tangent_t > &v, const Eigen::MatrixBase< Jacobian_t > &J) const
Definition: special-euclidean.hpp:389
pinocchio::SE3
SE3Tpl< double, 0 > SE3
Definition: src/spatial/fwd.hpp:53
pinocchio::SpecialEuclideanOperationTpl< 2, _Scalar, _Options >::log
static void log(const Eigen::MatrixBase< Matrix2Like > &R, const Eigen::MatrixBase< Vector2Like > &p, const Eigen::MatrixBase< TangentVector > &v)
Definition: special-euclidean.hpp:130
pinocchio::SpecialEuclideanOperationTpl< 3, _Scalar, _Options >::Quaternion_t
Eigen::Quaternion< Scalar, Options > Quaternion_t
Definition: special-euclidean.hpp:505
pinocchio::SpecialEuclideanOperationTpl< 2, _Scalar, _Options >::dIntegrate_dq_impl
static void dIntegrate_dq_impl(const Eigen::MatrixBase< Config_t > &, const Eigen::MatrixBase< Tangent_t > &v, const Eigen::MatrixBase< JacobianOut_t > &J, const AssignmentOperatorType op=SETTO)
Definition: special-euclidean.hpp:296

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