pinocchio  3.7.0
A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
 
Loading...
Searching...
No Matches
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
22namespace pinocchio
23{
24 template<int Dim, typename Scalar, int Options = 0>
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(quat0));
603 ConstQuaternionMap_t quat1(q1.derived().template tail<4>().data());
604 assert(quaternion::isNormalized(quat1));
605
606 typedef Eigen::Matrix<Scalar, 3, 1, PINOCCHIO_EIGEN_PLAIN_TYPE(Tangent_t)::Options> Vector3;
607 const Vector3 dv_pre = q1.derived().template head<3>() - q0.derived().template head<3>();
608 const Vector3 dv = quat0.conjugate() * dv_pre;
609 PINOCCHIO_EIGEN_CONST_CAST(Tangent_t, d).noalias() =
610 log6(quat0.conjugate() * quat1, dv).toVector();
611 }
612
615 template<ArgumentPosition arg, class ConfigL_t, class ConfigR_t, class JacobianOut_t>
616 static void dDifference_impl(
617 const Eigen::MatrixBase<ConfigL_t> & q0,
618 const Eigen::MatrixBase<ConfigR_t> & q1,
619 const Eigen::MatrixBase<JacobianOut_t> & J)
620 {
621 typedef typename SE3::Vector3 Vector3;
622
623 ConstQuaternionMap_t quat0(q0.derived().template tail<4>().data());
624 assert(quaternion::isNormalized(quat0));
625 ConstQuaternionMap_t quat1(q1.derived().template tail<4>().data());
626 assert(quaternion::isNormalized(quat1));
627
628 const Vector3 dv_pre = q1.derived().template head<3>() - q0.derived().template head<3>();
629 const Vector3 trans = quat0.conjugate() * dv_pre;
630
631 const Quaternion_t quat_diff = quat0.conjugate() * quat1;
632
633 const SE3 M(quat_diff, trans);
634
635 if (arg == ARG0)
636 {
637 PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
638 PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED
639 JacobianMatrix_t J1;
640 Jlog6(M, J1);
641 PINOCCHIO_COMPILER_DIAGNOSTIC_POP
642
643 const Vector3 p1_p0 = quat1.conjugate() * dv_pre;
644
645 JacobianOut_t & J0 = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, J);
646 J0.template bottomRightCorner<3, 3>() = J0.template topLeftCorner<3, 3>() =
647 -M.rotation().transpose();
648 J0.template topRightCorner<3, 3>().noalias() =
649 skew(p1_p0) * M.rotation().transpose(); // = R1.T * skew(q1_t - q0_t) * R0;
650 J0.template bottomLeftCorner<3, 3>().setZero();
651 J0.applyOnTheLeft(J1);
652 }
653 else if (arg == ARG1)
654 {
655 Jlog6(M, J);
656 }
657 }
658
659 template<class ConfigIn_t, class Velocity_t, class ConfigOut_t>
660 static void integrate_impl(
661 const Eigen::MatrixBase<ConfigIn_t> & q,
662 const Eigen::MatrixBase<Velocity_t> & v,
663 const Eigen::MatrixBase<ConfigOut_t> & qout)
664 {
665 ConfigOut_t & out = PINOCCHIO_EIGEN_CONST_CAST(ConfigOut_t, qout);
666 Quaternion_t const quat(q.derived().template tail<4>());
667 assert(quaternion::isNormalized(quat));
668 QuaternionMap_t res_quat(out.template tail<4>().data());
669
670 using internal::if_then_else;
671
672 typedef typename ConfigOut_t::Scalar Scalar;
673 enum
674 {
675 Options = PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigOut_t)::Options
676 };
677
678 Eigen::Matrix<Scalar, 7, 1, Options> expv;
679 quaternion::exp6(v, expv);
680
681 out.template head<3>() = (quat * expv.template head<3>()) + q.derived().template head<3>();
682
683 ConstQuaternionMap_t quat1(expv.template tail<4>().data());
684 res_quat = quat * quat1;
685
686 const Scalar dot_product = res_quat.dot(quat);
687 for (Eigen::DenseIndex k = 0; k < 4; ++k)
688 {
689 res_quat.coeffs().coeffRef(k) = if_then_else(
690 internal::LT, dot_product, Scalar(0), static_cast<Scalar>(-res_quat.coeffs().coeff(k)),
691 res_quat.coeffs().coeff(k));
692 }
693
694 // Norm of qs might be epsilon-different to 1, so M1.rotation might be epsilon-different to a
695 // rotation matrix. It is then safer to re-normalized after converting M1.rotation to
696 // quaternion.
697 quaternion::firstOrderNormalize(res_quat);
698 assert(quaternion::isNormalized(res_quat));
699 }
700
701 template<class Config_t, class Jacobian_t>
702 static void integrateCoeffWiseJacobian_impl(
703 const Eigen::MatrixBase<Config_t> & q, const Eigen::MatrixBase<Jacobian_t> & J)
704 {
705 assert(J.rows() == nq() && J.cols() == nv() && "J is not of the right dimension");
706
707 typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(Config_t) ConfigPlainType;
708 typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(Jacobian_t) JacobianPlainType;
709 typedef typename ConfigPlainType::Scalar Scalar;
710
711 Jacobian_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t, J);
712 Jout.setZero();
713
714 ConstQuaternionMap_t quat_map(q.derived().template tail<4>().data());
715 assert(quaternion::isNormalized(quat_map));
716 Jout.template topLeftCorner<3, 3>() = quat_map.toRotationMatrix();
717 // Jexp3(quat,Jout.template bottomRightCorner<4,3>());
718
719 typedef Eigen::Matrix<Scalar, 4, 3, JacobianPlainType::Options | Eigen::RowMajor> Jacobian43;
720 typedef SE3Tpl<Scalar, ConfigPlainType::Options> SE3;
721 PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
722 PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED
723 Jacobian43 Jexp3QuatCoeffWise;
724
725 Scalar theta;
726 typename SE3::Vector3 v = quaternion::log3(quat_map, theta);
727 quaternion::Jexp3CoeffWise(v, Jexp3QuatCoeffWise);
728 typename SE3::Matrix3 Jlog;
729 Jlog3(theta, v, Jlog);
730 PINOCCHIO_COMPILER_DIAGNOSTIC_POP
731
732 // std::cout << "Jexp3QuatCoeffWise\n" << Jexp3QuatCoeffWise << std::endl;
733 // std::cout << "Jlog\n" << Jlog << std::endl;
734
735 // if(quat_map.w() >= 0.) // comes from the log3 for quaternions which may change the
736 // sign.
737 if (quat_map.coeffs()[3] >= Scalar(0)) // comes from the log3 for quaternions which may change
738 // the sign.
739 PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t, J).template bottomRightCorner<4, 3>().noalias() =
740 Jexp3QuatCoeffWise * Jlog;
741 else
742 PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t, J).template bottomRightCorner<4, 3>().noalias() =
743 -Jexp3QuatCoeffWise * Jlog;
744 }
745
746 template<class Config_t, class Tangent_t, class JacobianOut_t>
747 static void dIntegrate_dq_impl(
748 const Eigen::MatrixBase<Config_t> & /*q*/,
749 const Eigen::MatrixBase<Tangent_t> & v,
750 const Eigen::MatrixBase<JacobianOut_t> & J,
751 const AssignmentOperatorType op = SETTO)
752 {
753 JacobianOut_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, J);
754
755 switch (op)
756 {
757 case SETTO:
758 Jout = exp6(MotionRef<const Tangent_t>(v.derived())).toDualActionMatrix().transpose();
759 break;
760 case ADDTO:
761 Jout += exp6(MotionRef<const Tangent_t>(v.derived())).toDualActionMatrix().transpose();
762 break;
763 case RMTO:
764 Jout -= exp6(MotionRef<const Tangent_t>(v.derived())).toDualActionMatrix().transpose();
765 break;
766 default:
767 assert(false && "Wrong Op requesed value");
768 break;
769 }
770 }
771
772 template<class Config_t, class Tangent_t, class JacobianOut_t>
773 static void dIntegrate_dv_impl(
774 const Eigen::MatrixBase<Config_t> & /*q*/,
775 const Eigen::MatrixBase<Tangent_t> & v,
776 const Eigen::MatrixBase<JacobianOut_t> & J,
777 const AssignmentOperatorType op = SETTO)
778 {
779 switch (op)
780 {
781 case SETTO:
782 Jexp6<SETTO>(MotionRef<const Tangent_t>(v.derived()), J.derived());
783 break;
784 case ADDTO:
785 Jexp6<ADDTO>(MotionRef<const Tangent_t>(v.derived()), J.derived());
786 break;
787 case RMTO:
788 Jexp6<RMTO>(MotionRef<const Tangent_t>(v.derived()), J.derived());
789 break;
790 default:
791 assert(false && "Wrong Op requesed value");
792 break;
793 }
794 }
795
796 template<class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
797 static void dIntegrateTransport_dq_impl(
798 const Eigen::MatrixBase<Config_t> & /*q*/,
799 const Eigen::MatrixBase<Tangent_t> & v,
800 const Eigen::MatrixBase<JacobianIn_t> & Jin,
801 const Eigen::MatrixBase<JacobianOut_t> & J_out)
802 {
803 JacobianOut_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, J_out);
804 Eigen::Matrix<Scalar, 6, 6> Jtmp6;
805 Jtmp6 = exp6(MotionRef<const Tangent_t>(v.derived())).toDualActionMatrix().transpose();
806
807 Jout.template topRows<3>().noalias() =
808 Jtmp6.template topLeftCorner<3, 3>() * Jin.template topRows<3>();
809 Jout.template topRows<3>().noalias() +=
810 Jtmp6.template topRightCorner<3, 3>() * Jin.template bottomRows<3>();
811 Jout.template bottomRows<3>().noalias() =
812 Jtmp6.template bottomRightCorner<3, 3>() * Jin.template bottomRows<3>();
813 }
814
815 template<class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
816 static void dIntegrateTransport_dv_impl(
817 const Eigen::MatrixBase<Config_t> & /*q*/,
818 const Eigen::MatrixBase<Tangent_t> & v,
819 const Eigen::MatrixBase<JacobianIn_t> & Jin,
820 const Eigen::MatrixBase<JacobianOut_t> & J_out)
821 {
822 JacobianOut_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, J_out);
823 PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
824 PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED
825 Eigen::Matrix<Scalar, 6, 6> Jtmp6;
826 Jexp6<SETTO>(MotionRef<const Tangent_t>(v.derived()), Jtmp6);
827 PINOCCHIO_COMPILER_DIAGNOSTIC_POP
828
829 Jout.template topRows<3>().noalias() =
830 Jtmp6.template topLeftCorner<3, 3>() * Jin.template topRows<3>();
831 Jout.template topRows<3>().noalias() +=
832 Jtmp6.template topRightCorner<3, 3>() * Jin.template bottomRows<3>();
833 Jout.template bottomRows<3>().noalias() =
834 Jtmp6.template bottomRightCorner<3, 3>() * Jin.template bottomRows<3>();
835 }
836
837 template<class Config_t, class Tangent_t, class Jacobian_t>
838 static void dIntegrateTransport_dq_impl(
839 const Eigen::MatrixBase<Config_t> & /*q*/,
840 const Eigen::MatrixBase<Tangent_t> & v,
841 const Eigen::MatrixBase<Jacobian_t> & J_out)
842 {
843 Jacobian_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t, J_out);
844 Eigen::Matrix<Scalar, 6, 6> Jtmp6;
845 Jtmp6 = exp6(MotionRef<const Tangent_t>(v.derived())).toDualActionMatrix().transpose();
846
847 // Aliasing
848 Jout.template topRows<3>() =
849 Jtmp6.template topLeftCorner<3, 3>() * Jout.template topRows<3>();
850 Jout.template topRows<3>().noalias() +=
851 Jtmp6.template topRightCorner<3, 3>() * Jout.template bottomRows<3>();
852 Jout.template bottomRows<3>() =
853 Jtmp6.template bottomRightCorner<3, 3>() * Jout.template bottomRows<3>();
854 }
855
856 template<class Config_t, class Tangent_t, class Jacobian_t>
857 static void dIntegrateTransport_dv_impl(
858 const Eigen::MatrixBase<Config_t> & /*q*/,
859 const Eigen::MatrixBase<Tangent_t> & v,
860 const Eigen::MatrixBase<Jacobian_t> & J_out)
861 {
862 Jacobian_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t, J_out);
863 PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
864 PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED
865 Eigen::Matrix<Scalar, 6, 6> Jtmp6;
866 Jexp6<SETTO>(MotionRef<const Tangent_t>(v.derived()), Jtmp6);
867 PINOCCHIO_COMPILER_DIAGNOSTIC_POP
868
869 Jout.template topRows<3>() =
870 Jtmp6.template topLeftCorner<3, 3>() * Jout.template topRows<3>();
871 Jout.template topRows<3>().noalias() +=
872 Jtmp6.template topRightCorner<3, 3>() * Jout.template bottomRows<3>();
873 Jout.template bottomRows<3>() =
874 Jtmp6.template bottomRightCorner<3, 3>() * Jout.template bottomRows<3>();
875 }
876
877 template<class ConfigL_t, class ConfigR_t>
878 static Scalar squaredDistance_impl(
879 const Eigen::MatrixBase<ConfigL_t> & q0, const Eigen::MatrixBase<ConfigR_t> & q1)
880 {
881 PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
882 PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED
883 TangentVector_t t;
884 difference_impl(q0, q1, t);
885 PINOCCHIO_COMPILER_DIAGNOSTIC_POP
886 return t.squaredNorm();
887 }
888
889 template<class Config_t>
890 static void normalize_impl(const Eigen::MatrixBase<Config_t> & qout)
891 {
892 pinocchio::normalize(qout.const_cast_derived().template tail<4>());
893 }
894
895 template<class Config_t>
896 static bool isNormalized_impl(const Eigen::MatrixBase<Config_t> & qin, const Scalar & prec)
897 {
898 Scalar norm = Scalar(qin.template tail<4>().norm());
899 using std::abs;
900 return abs(norm - Scalar(1.0)) < prec;
901 }
902
903 template<class Config_t>
904 static void random_impl(const Eigen::MatrixBase<Config_t> & qout)
905 {
906 R3crossSO3_t().random(qout);
907 }
908
909 template<class ConfigL_t, class ConfigR_t, class ConfigOut_t>
910 static void randomConfiguration_impl(
911 const Eigen::MatrixBase<ConfigL_t> & lower,
912 const Eigen::MatrixBase<ConfigR_t> & upper,
913 const Eigen::MatrixBase<ConfigOut_t> & qout)
914 {
915 R3crossSO3_t().randomConfiguration(lower, upper, qout);
916 }
917
918 template<class ConfigL_t, class ConfigR_t>
919 static bool isSameConfiguration_impl(
920 const Eigen::MatrixBase<ConfigL_t> & q0,
921 const Eigen::MatrixBase<ConfigR_t> & q1,
922 const Scalar & prec)
923 {
924 return R3crossSO3_t().isSameConfiguration(q0, q1, prec);
925 }
926 }; // struct SpecialEuclideanOperationTpl<3>
927
928} // namespace pinocchio
929
930#endif // ifndef __pinocchio_multibody_liegroup_special_euclidean_operation_hpp__
pinocchio::quaternion::isNormalized
bool isNormalized(const Eigen::QuaternionBase< Quaternion > &quat, const typename Quaternion::Coefficients::RealScalar &prec)
Check whether the input quaternion is Normalized within the given precision.
Definition quaternion.hpp:230
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::quaternion::firstOrderNormalize
void firstOrderNormalize(const Eigen::QuaternionBase< D > &q)
Definition quaternion.hpp:90
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::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
Main pinocchio namespace.
Definition treeview.dox:11
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.
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::normalize
void normalize(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorType > &qout)
Normalize a configuration vector.
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::SINCOS
void SINCOS(const S1 &a, S2 *sa, S3 *ca)
Computes sin/cos values of a given input scalar.
Definition sincos.hpp:27
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::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::name
std::string name(const LieGroupGenericTpl< LieGroupCollection > &lg)
Visit a LieGroupVariant to get the name of it.
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::Jlog6
void Jlog6(const SE3Tpl< Scalar, Options > &M, const Eigen::MatrixBase< Matrix6Like > &Jlog)
Derivative of log6.
Definition explog.hpp:668
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::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::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:616
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