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