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