explog.cpp
Go to the documentation of this file.
1 //
2 // Copyright (c) 2016-2021 CNRS INRIA
3 // Copyright (c) 2015 Wandercraft, 86 rue de Paris 91400 Orsay, France.
4 //
5 
6 #include "pinocchio/spatial/explog.hpp"
7 
8 #include <iostream>
9 #include <boost/test/unit_test.hpp>
10 #include <boost/utility/binary.hpp>
11 
12 #include "utils/macros.hpp"
13 
14 using namespace pinocchio;
15 
16 BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)
17 
18 BOOST_AUTO_TEST_CASE(exp)
19 {
20  SE3 M(SE3::Random());
21  Motion v(Motion::Random());
22  v.linear().setZero();
23 
24  SE3::Matrix3 R = exp3(v.angular());
25  BOOST_CHECK(
26  R.isApprox(Eigen::AngleAxis<double>(v.angular().norm(), v.angular().normalized()).matrix()));
27 
28  SE3::Matrix3 R0 = exp3(SE3::Vector3::Zero());
29  BOOST_CHECK(R0.isIdentity());
30 
31 // check the NAN case
32 #ifdef NDEBUG
33  Motion::Vector3 vec3_nan(Motion::Vector3::Constant(NAN));
34  SE3::Matrix3 R_nan = exp3(vec3_nan);
35  BOOST_CHECK(R_nan != R_nan);
36 #endif
37 
38  M = exp6(v);
39 
40  BOOST_CHECK(R.isApprox(M.rotation()));
41 
42  Eigen::Matrix<double, 7, 1> q0;
43  q0 << 0., 0., 0., 0., 0., 0., 1.;
44  Eigen::Matrix<double, 7, 1> q = quaternion::exp6(v);
45  Eigen::Quaterniond quat0(q.tail<4>());
46  BOOST_CHECK(R.isApprox(quat0.matrix()));
47 
48 // check the NAN case
49 #ifdef NDEBUG
50  Motion::Vector6 vec6_nan(Motion::Vector6::Constant(NAN));
51  SE3 M_nan = exp6(vec6_nan);
52  BOOST_CHECK(M_nan != M_nan);
53 #endif
54 
55  R = exp3(SE3::Vector3::Zero());
56  BOOST_CHECK(R.isIdentity());
57 
58  // Quaternion
59  Eigen::Quaterniond quat;
60  quaternion::exp3(v.angular(), quat);
61  BOOST_CHECK(quat.toRotationMatrix().isApprox(M.rotation()));
62 
63  quaternion::exp3(SE3::Vector3::Zero(), quat);
64  BOOST_CHECK(quat.toRotationMatrix().isIdentity());
65  BOOST_CHECK(quat.vec().isZero() && quat.coeffs().tail<1>().isOnes());
66 
67  // Check QuaternionMap
68  Eigen::Vector4d vec4;
69  Eigen::QuaternionMapd quat_map(vec4.data());
70  quaternion::exp3(v.angular(), quat_map);
71  BOOST_CHECK(quat_map.toRotationMatrix().isApprox(M.rotation()));
72 }
73 
74 BOOST_AUTO_TEST_CASE(renorm_rotation)
75 {
76  SE3 M0, M1;
77  SE3::Matrix3 R0, R1;
78  SE3::Matrix3 R_normed;
79  SE3::Matrix3 Id(SE3::Matrix3::Identity());
80  SE3::Vector3 vals;
81  double tr0, tr;
82  const size_t num_tries = 20;
83 
84  for (size_t i = 0; i < num_tries; i++)
85  {
86  M0.setRandom();
87  M1 = M0.actInv(M0);
88  R1 = M1.rotation();
89  R_normed = pinocchio::renormalize_rotation_matrix(R1);
90  BOOST_CHECK((R_normed.transpose() * R_normed).isApprox(Id));
91  tr0 = R1.trace();
92 
93  tr = R_normed.trace();
94  vals = 2. * R_normed.diagonal().array() - tr + 1.;
95  }
96 }
97 
98 BOOST_AUTO_TEST_CASE(log)
99 {
100  SE3 M(SE3::Identity());
101  Motion v(Motion::Random());
102  v.linear().setZero();
103 
104  SE3::Vector3 omega = log3(M.rotation());
105  BOOST_CHECK(omega.isZero());
106 
107  // check the NAN case
108 #ifdef NDEBUG
109  SE3::Matrix3 mat3_nan(SE3::Matrix3::Constant(NAN));
110  SE3::Vector3 omega_nan = log3(mat3_nan);
111  BOOST_CHECK(omega_nan != omega_nan);
112 #endif
113 
114  M.setRandom();
115  M.translation().setZero();
116 
117  v = log6(M);
118  omega = log3(M.rotation());
119  BOOST_CHECK(omega.isApprox(v.angular()));
120 
121 // check the NAN case
122 #ifdef NDEBUG
123  SE3::Matrix4 mat4_nan(SE3::Matrix4::Constant(NAN));
124  Motion::Vector6 v_nan = log6(mat4_nan);
125  BOOST_CHECK(v_nan != v_nan);
126 #endif
127 
128  // Quaternion
129  Eigen::Quaterniond quat(SE3::Matrix3::Identity());
130  omega = quaternion::log3(quat);
131  BOOST_CHECK(omega.isZero());
132 
133  for (int k = 0; k < 1e3; ++k)
134  {
135  SE3::Vector3 w;
136  w.setRandom();
137  quaternion::exp3(w, quat);
138  SE3::Matrix3 rot = exp3(w);
139 
140  BOOST_CHECK(quat.toRotationMatrix().isApprox(rot));
141  double theta;
142  omega = quaternion::log3(quat, theta);
143  const double PI_value = PI<double>();
144  BOOST_CHECK(omega.norm() <= PI_value);
145  double theta_ref;
146  SE3::Vector3 omega_ref = log3(quat.toRotationMatrix(), theta_ref);
147 
148  BOOST_CHECK(omega.isApprox(omega_ref));
149  }
150 
151  // Check QuaternionMap
152  Eigen::Vector4d vec4;
153  Eigen::QuaternionMapd quat_map(vec4.data());
154  quat_map = SE3::Random().rotation();
155  BOOST_CHECK(quaternion::log3(quat_map).isApprox(log3(quat_map.toRotationMatrix())));
156 }
157 
158 BOOST_AUTO_TEST_CASE(explog3)
159 {
160  SE3 M(SE3::Random());
161  SE3::Matrix3 M_res = exp3(log3(M.rotation()));
162  BOOST_CHECK(M_res.isApprox(M.rotation()));
163 
164  Motion::Vector3 v;
165  v.setRandom();
166  Motion::Vector3 v_res = log3(exp3(v));
167  BOOST_CHECK(v_res.isApprox(v));
168 }
169 
170 BOOST_AUTO_TEST_CASE(explog3_quaternion)
171 {
172  SE3 M(SE3::Random());
173  Eigen::Quaterniond quat;
174  quat = M.rotation();
175  Eigen::Quaterniond quat_res;
176  quaternion::exp3(quaternion::log3(quat), quat_res);
177  BOOST_CHECK(quat_res.isApprox(quat) || quat_res.coeffs().isApprox(-quat.coeffs()));
178 
179  Motion::Vector3 v;
180  v.setRandom();
181  quaternion::exp3(v, quat);
182  BOOST_CHECK(quaternion::log3(quat).isApprox(v));
183 
184  SE3::Matrix3 R_next = M.rotation() * exp3(v);
185  Motion::Vector3 v_est = log3(M.rotation().transpose() * R_next);
186  BOOST_CHECK(v_est.isApprox(v));
187 
188  SE3::Quaternion quat_v;
189  quaternion::exp3(v, quat_v);
190  SE3::Quaternion quat_next = quat * quat_v;
191  v_est = quaternion::log3(quat.conjugate() * quat_next);
192  BOOST_CHECK(v_est.isApprox(v));
193 }
194 
195 BOOST_AUTO_TEST_CASE(Jlog3_fd)
196 {
197  SE3 M(SE3::Random());
198  SE3::Matrix3 R(M.rotation());
199 
200  PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
201  PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED
202  SE3::Matrix3 Jfd, Jlog;
203  Jlog3(R, Jlog);
204  PINOCCHIO_COMPILER_DIAGNOSTIC_POP
205  Jfd.setZero();
206 
207  Motion::Vector3 dR;
208  dR.setZero();
209  const double eps = 1e-8;
210  for (int i = 0; i < 3; ++i)
211  {
212  dR[i] = eps;
213  SE3::Matrix3 R_dR_plus = R * exp3(dR);
214  SE3::Matrix3 R_dR_minus = R * exp3(-dR);
215  Jfd.col(i) = (log3(R_dR_plus) - log3(R_dR_minus)) / (2 * eps);
216  dR[i] = 0;
217  }
218  BOOST_CHECK(Jfd.isApprox(Jlog, std::sqrt(eps)));
219 }
220 
221 BOOST_AUTO_TEST_CASE(Jexp3_fd)
222 {
223  SE3 M(SE3::Random());
224  SE3::Matrix3 R(M.rotation());
225 
226  Motion::Vector3 v = log3(R);
227 
228  PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
229  PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED
230  SE3::Matrix3 Jexp_fd, Jexp;
231 
232  Jexp3(Motion::Vector3::Zero(), Jexp);
233  BOOST_CHECK(Jexp.isIdentity());
234 
235  Jexp3(v, Jexp);
236  PINOCCHIO_COMPILER_DIAGNOSTIC_POP
237 
238  Motion::Vector3 dv;
239  dv.setZero();
240  const double eps = 1e-3;
241  for (int i = 0; i < 3; ++i)
242  {
243  dv[i] = eps;
244  SE3::Matrix3 R_next = exp3(v + dv);
245  SE3::Matrix3 R_prev = exp3(v - dv);
246  SE3::Matrix3 Rpn = R_prev.transpose() * R_next;
247  Jexp_fd.col(i) = log3(Rpn) / (2. * eps);
248  dv[i] = 0;
249  }
250  BOOST_CHECK(Jexp_fd.isApprox(Jexp, std::sqrt(eps)));
251 }
252 
253 template<typename QuaternionLike, typename Matrix43Like>
254 void Jexp3QuatLocal(
255  const Eigen::QuaternionBase<QuaternionLike> & quat, const Eigen::MatrixBase<Matrix43Like> & Jexp)
256 {
257  Matrix43Like & Jout = PINOCCHIO_EIGEN_CONST_CAST(Matrix43Like, Jexp);
258 
259  skew(0.5 * quat.vec(), Jout.template topRows<3>());
260  Jout.template topRows<3>().diagonal().array() += 0.5 * quat.w();
261  Jout.template bottomRows<1>() = -0.5 * quat.vec().transpose();
262 }
263 
264 BOOST_AUTO_TEST_CASE(Jexp3_quat_fd)
265 {
266  typedef double Scalar;
267  SE3::Vector3 w;
268  w.setRandom();
269  SE3::Quaternion quat;
270  quaternion::exp3(w, quat);
271 
272  typedef Eigen::Matrix<Scalar, 4, 3> Matrix43;
273  PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
274  PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED
275  Matrix43 Jexp3, Jexp3_fd;
276  quaternion::Jexp3CoeffWise(w, Jexp3);
277  PINOCCHIO_COMPILER_DIAGNOSTIC_POP
278  SE3::Vector3 dw;
279  dw.setZero();
280  const double eps = 1e-8;
281 
282  for (int i = 0; i < 3; ++i)
283  {
284  dw[i] = eps;
285  SE3::Quaternion quat_plus;
286  quaternion::exp3(w + dw, quat_plus);
287  Jexp3_fd.col(i) = (quat_plus.coeffs() - quat.coeffs()) / eps;
288  dw[i] = 0;
289  }
290  BOOST_CHECK(Jexp3.isApprox(Jexp3_fd, sqrt(eps)));
291 
292  PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
293  PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED
294  SE3::Matrix3 Jlog;
295  pinocchio::Jlog3(quat.toRotationMatrix(), Jlog);
296 
297  Matrix43 Jexp_quat_local;
298  Jexp3QuatLocal(quat, Jexp_quat_local);
299  PINOCCHIO_COMPILER_DIAGNOSTIC_POP
300 
301  Matrix43 Jcompositon = Jexp3 * Jlog;
302  BOOST_CHECK(Jcompositon.isApprox(Jexp_quat_local));
303  // std::cout << "Jcompositon\n" << Jcompositon << std::endl;
304  // std::cout << "Jexp_quat_local\n" << Jexp_quat_local << std::endl;
305 
306  // Arount zero
307  w.setZero();
308  w.fill(1e-6);
309  quaternion::exp3(w, quat);
310  quaternion::Jexp3CoeffWise(w, Jexp3);
311  for (int i = 0; i < 3; ++i)
312  {
313  dw[i] = eps;
314  SE3::Quaternion quat_plus;
315  quaternion::exp3(w + dw, quat_plus);
316  Jexp3_fd.col(i) = (quat_plus.coeffs() - quat.coeffs()) / eps;
317  dw[i] = 0;
318  }
319  BOOST_CHECK(Jexp3.isApprox(Jexp3_fd, sqrt(eps)));
320 }
321 
322 BOOST_AUTO_TEST_CASE(Jexp3_quat)
323 {
324  SE3 M(SE3::Random());
325  SE3::Quaternion quat(M.rotation());
326 
327  Motion dv(Motion::Zero());
328  const double eps = 1e-8;
329 
330  typedef Eigen::Matrix<double, 7, 6> Matrix76;
331  Matrix76 Jexp6_fd, Jexp6_quat;
332  Jexp6_quat.setZero();
333  typedef Eigen::Matrix<double, 4, 3> Matrix43;
334  PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
335  PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED
336  Matrix43 Jexp3_quat;
337  Jexp3QuatLocal(quat, Jexp3_quat);
338  PINOCCHIO_COMPILER_DIAGNOSTIC_POP
339  SE3 M_next;
340 
341  Jexp6_quat.middleRows<3>(Motion::LINEAR).middleCols<3>(Motion::LINEAR) = M.rotation();
342  Jexp6_quat.middleRows<4>(Motion::ANGULAR).middleCols<3>(Motion::ANGULAR) =
343  Jexp3_quat; // * Jlog6_SE3.middleRows<3>(Motion::ANGULAR);
344  for (int i = 0; i < 6; ++i)
345  {
346  dv.toVector()[i] = eps;
347  M_next = M * exp6(dv);
348  const SE3::Quaternion quat_next(M_next.rotation());
349  Jexp6_fd.middleRows<3>(Motion::LINEAR).col(i) = (M_next.translation() - M.translation()) / eps;
350  Jexp6_fd.middleRows<4>(Motion::ANGULAR).col(i) = (quat_next.coeffs() - quat.coeffs()) / eps;
351  dv.toVector()[i] = 0.;
352  }
353 
354  BOOST_CHECK(Jexp6_quat.isApprox(Jexp6_fd, sqrt(eps)));
355 }
356 
357 BOOST_AUTO_TEST_CASE(Jexplog3)
358 {
359  Motion v(Motion::Random());
360 
361  PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
362  PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED
363  Eigen::Matrix3d R(exp3(v.angular())), Jexp, Jlog;
364  Jexp3(v.angular(), Jexp);
365  Jlog3(R, Jlog);
366  PINOCCHIO_COMPILER_DIAGNOSTIC_POP
367 
368  BOOST_CHECK((Jlog * Jexp).isIdentity());
369 
370  SE3 M(SE3::Random());
371  R = M.rotation();
372  v.angular() = log3(R);
373  Jlog3(R, Jlog);
374  Jexp3(v.angular(), Jexp);
375 
376  BOOST_CHECK((Jexp * Jlog).isIdentity());
377 }
378 
379 BOOST_AUTO_TEST_CASE(Jlog3_quat)
380 {
381  SE3::Vector3 w;
382  w.setRandom();
383  SE3::Quaternion quat;
384  quaternion::exp3(w, quat);
385 
386  SE3::Matrix3 R(quat.toRotationMatrix());
387 
388  PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
389  PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED
390  SE3::Matrix3 res, res_ref;
391  quaternion::Jlog3(quat, res);
392  Jlog3(R, res_ref);
393  PINOCCHIO_COMPILER_DIAGNOSTIC_POP
394 
395  BOOST_CHECK(res.isApprox(res_ref));
396 }
397 
398 BOOST_AUTO_TEST_CASE(explog6)
399 {
400  SE3 M(SE3::Random());
401  SE3 M_res = exp6(log6(M));
402  BOOST_CHECK(M_res.isApprox(M));
403 
404  Motion v(Motion::Random());
405  Motion v_res = log6(exp6(v));
406  BOOST_CHECK(v_res.toVector().isApprox(v.toVector()));
407 }
408 
409 BOOST_AUTO_TEST_CASE(Jlog6_fd)
410 {
411  SE3 M(SE3::Random());
412 
413  PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
414  PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED
415  SE3::Matrix6 Jfd, Jlog;
416  Jlog6(M, Jlog);
417  PINOCCHIO_COMPILER_DIAGNOSTIC_POP
418  Jfd.setZero();
419 
420  Motion dM;
421  dM.setZero();
422  double step = 1e-8;
423  for (int i = 0; i < 6; ++i)
424  {
425  dM.toVector()[i] = step;
426  SE3 M_dM = M * exp6(dM);
427  Jfd.col(i) = (log6(M_dM).toVector() - log6(M).toVector()) / step;
428  dM.toVector()[i] = 0;
429  }
430 
431  BOOST_CHECK(Jfd.isApprox(Jlog, sqrt(step)));
432 
433  SE3::Matrix6 Jlog2 = Jlog6(M);
434  BOOST_CHECK(Jlog2.isApprox(Jlog));
435 }
436 
437 BOOST_AUTO_TEST_CASE(Jlog6_singular)
438 {
439  for (size_t i = 0; i < 15; i++)
440  {
441  SE3 M0;
442  if (i == 0)
443  {
444  M0 = SE3::Identity();
445  }
446  else
447  {
448  M0 = SE3::Random();
449  }
450  SE3 dM(M0.actInv(M0));
451 
452  BOOST_CHECK(dM.isApprox(SE3::Identity()));
453  SE3::Matrix6 J0(SE3::Matrix6::Identity());
454 
455  SE3::Matrix6 J_val = Jlog6(dM);
456  BOOST_CHECK(J0.isApprox(J_val));
457  }
458 }
459 
460 BOOST_AUTO_TEST_CASE(Jexp6_fd)
461 {
462  SE3 M(SE3::Random());
463 
464  const Motion v = log6(M);
465 
466  SE3::Matrix6 Jexp_fd, Jexp;
467 
468  Jexp6(Motion::Zero(), Jexp);
469  BOOST_CHECK(Jexp.isIdentity());
470 
471  Jexp6(v, Jexp);
472 
473  Motion::Vector6 dv;
474  dv.setZero();
475  const double eps = 1e-8;
476  for (int i = 0; i < 6; ++i)
477  {
478  dv[i] = eps;
479  SE3 M_next = exp6(v + Motion(dv));
480  Jexp_fd.col(i) = log6(M.actInv(M_next)).toVector() / eps;
481  dv[i] = 0;
482  }
483  BOOST_CHECK(Jexp_fd.isApprox(Jexp, std::sqrt(eps)));
484 
485  SE3::Matrix6 Jexp2 = Jexp6(v);
486  BOOST_CHECK(Jexp2.isApprox(Jexp));
487 }
488 
489 BOOST_AUTO_TEST_CASE(Jlog6_of_product_fd)
490 {
491  SE3 Ma(SE3::Random());
492  SE3 Mb(SE3::Random());
493 
494  PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
495  PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED
496  SE3::Matrix6 Jlog, Ja, Jb, Jfda, Jfdb;
497  Jlog6(Ma.inverse() * Mb, Jlog);
498  PINOCCHIO_COMPILER_DIAGNOSTIC_POP
499  Ja = -Jlog * (Ma.inverse() * Mb).toActionMatrixInverse();
500  Jb = Jlog;
501  Jfda.setZero();
502  Jfdb.setZero();
503 
504  Motion dM;
505  dM.setZero();
506  double step = 1e-8;
507  for (int i = 0; i < 6; ++i)
508  {
509  dM.toVector()[i] = step;
510  Jfda.col(i) =
511  (log6((Ma * exp6(dM)).inverse() * Mb).toVector() - log6(Ma.inverse() * Mb).toVector()) / step;
512  Jfdb.col(i) =
513  (log6(Ma.inverse() * Mb * exp6(dM)).toVector() - log6(Ma.inverse() * Mb).toVector()) / step;
514  dM.toVector()[i] = 0;
515  }
516 
517  BOOST_CHECK(Jfda.isApprox(Ja, sqrt(step)));
518  BOOST_CHECK(Jfdb.isApprox(Jb, sqrt(step)));
519 }
520 
521 BOOST_AUTO_TEST_CASE(Jexplog6)
522 {
523  Motion v(Motion::Random());
524 
525  SE3 M(exp6(v));
526  {
527  Eigen::Matrix<double, 6, 6, Eigen::RowMajor> Jexp, Jlog;
528  Jexp6(v, Jexp);
529  Jlog6(M, Jlog);
530 
531  BOOST_CHECK((Jlog * Jexp).isIdentity());
532  }
533 
534  M.setRandom();
535 
536  v = log6(M);
537  {
538  Eigen::Matrix<double, 6, 6> Jexp, Jlog;
539  Jlog6(M, Jlog);
540  Jexp6(v, Jexp);
541 
542  BOOST_CHECK((Jexp * Jlog).isIdentity());
543  }
544 }
545 
546 BOOST_AUTO_TEST_CASE(Hlog3_fd)
547 {
548  typedef SE3::Vector3 Vector3;
549  typedef SE3::Matrix3 Matrix3;
550  SE3::Quaternion q;
551  quaternion::uniformRandom(q);
552  Matrix3 R(q.matrix());
553 
554  // Hlog3(R, v) returns the matrix H * v
555  // We check that H * e_k matches the finite difference of Hlog3(R, e_k)
556  Vector3 dR;
557  dR.setZero();
558  double step = 1e-8;
559  for (int k = 0; k < 3; ++k)
560  {
561  Vector3 e_k(Vector3::Zero());
562  e_k[k] = 1.;
563 
564  Matrix3 Hlog_e_k;
565  Hlog3(R, e_k, Hlog_e_k);
566 
567  Matrix3 R_dR = R * exp3(step * e_k);
568  Matrix3 Jlog_R, Jlog_R_dR;
569  Jlog3(R, Jlog_R);
570  Jlog3(R_dR, Jlog_R_dR);
571 
572  Matrix3 Hlog_e_k_fd = (Jlog_R_dR - Jlog_R) / step;
573 
574  BOOST_CHECK(Hlog_e_k.isApprox(Hlog_e_k_fd, sqrt(step)));
575  }
576 }
577 
578 BOOST_AUTO_TEST_CASE(test_basic)
579 {
580  typedef pinocchio::SE3::Vector3 Vector3;
581  typedef pinocchio::SE3::Matrix3 Matrix3;
582  typedef Eigen::Matrix4d Matrix4;
583  typedef pinocchio::Motion::Vector6 Vector6;
584 
585  const double EPSILON = 1e-12;
586 
587  // exp3 and log3.
588  Vector3 v3(Vector3::Random());
589  Matrix3 R(pinocchio::exp3(v3));
590  BOOST_CHECK(R.transpose().isApprox(R.inverse(), EPSILON));
591  BOOST_CHECK_SMALL(R.determinant() - 1.0, EPSILON);
592  Vector3 v3FromLog(pinocchio::log3(R));
593  BOOST_CHECK(v3.isApprox(v3FromLog, EPSILON));
594 
595  // exp6 and log6.
596  pinocchio::Motion nu = pinocchio::Motion::Random();
597  pinocchio::SE3 m = pinocchio::exp6(nu);
598  BOOST_CHECK(m.rotation().transpose().isApprox(m.rotation().inverse(), EPSILON));
599  BOOST_CHECK_SMALL(m.rotation().determinant() - 1.0, EPSILON);
600  pinocchio::Motion nuFromLog(pinocchio::log6(m));
601  BOOST_CHECK(nu.linear().isApprox(nuFromLog.linear(), EPSILON));
602  BOOST_CHECK(nu.angular().isApprox(nuFromLog.angular(), EPSILON));
603 
604  Vector6 v6(Vector6::Random());
605  pinocchio::SE3 m2(pinocchio::exp6(v6));
606  BOOST_CHECK(m2.rotation().transpose().isApprox(m2.rotation().inverse(), EPSILON));
607  BOOST_CHECK_SMALL(m2.rotation().determinant() - 1.0, EPSILON);
608  Matrix4 M = m2.toHomogeneousMatrix();
609  pinocchio::Motion nu2FromLog(pinocchio::log6(M));
610  Vector6 v6FromLog(nu2FromLog.toVector());
611  BOOST_CHECK(v6.isApprox(v6FromLog, EPSILON));
612 }
613 
614 BOOST_AUTO_TEST_CASE(test_SE3_interpolate)
615 {
616  SE3 A = SE3::Random();
617  SE3 B = SE3::Random();
618 
619  SE3 A_bis = SE3::Interpolate(A, B, 0.);
620  BOOST_CHECK(A_bis.isApprox(A));
621  SE3 B_bis = SE3::Interpolate(A, B, 1.);
622  BOOST_CHECK(B_bis.isApprox(B));
623 
624  A_bis = SE3::Interpolate(A, A, 1.);
625  BOOST_CHECK(A_bis.isApprox(A));
626 
627  B_bis = SE3::Interpolate(B, B, 1.);
628  BOOST_CHECK(B_bis.isApprox(B));
629 
630  SE3 C = SE3::Interpolate(A, B, 0.5);
631  SE3 D = SE3::Interpolate(B, A, 0.5);
632  BOOST_CHECK(D.isApprox(C));
633 }
634 
635 BOOST_AUTO_TEST_CASE(test_Jlog6_robustness)
636 {
637  const int num_tests = 1e1;
638  for (int k = 0; k < num_tests; ++k)
639  {
640  const SE3 M = SE3::Random();
641  SE3::Matrix6 res = Jlog6(M.actInv(M));
642 
643  BOOST_CHECK(res.isIdentity());
644  }
645 }
646 
647 BOOST_AUTO_TEST_SUITE_END()
pinocchio::SE3Tpl< context::Scalar, context::Options >::Matrix4
traits< SE3Tpl >::Matrix4 Matrix4
Definition: spatial/se3-tpl.hpp:57
test-cpp2pybind11.m
m
Definition: test-cpp2pybind11.py:25
pinocchio::SE3Tpl< context::Scalar, context::Options >::Vector3
traits< SE3Tpl >::Vector3 Vector3
Definition: spatial/se3-tpl.hpp:55
pinocchio::Hlog3
void Hlog3(const Scalar &theta, const Eigen::MatrixBase< Vector3Like1 > &log, const Eigen::MatrixBase< Vector3Like2 > &v, const Eigen::MatrixBase< Matrix3Like > &vt_Hlog)
Definition: spatial/explog.hpp:273
pinocchio::SE3Tpl::inverse
SE3Tpl inverse() const
aXb = bXa.inverse()
Definition: spatial/se3-tpl.hpp:149
pinocchio::quaternion::uniformRandom
void uniformRandom(Eigen::QuaternionBase< Derived > &q)
Uniformly random quaternion sphere.
Definition: math/quaternion.hpp:114
autodiff-rnea.dv
dv
Definition: autodiff-rnea.py:27
pinocchio::SE3Tpl::setRandom
SE3Tpl & setRandom()
Definition: spatial/se3-tpl.hpp:159
quat
quat
B
B
pinocchio.explog.exp
def exp(x)
Definition: bindings/python/pinocchio/explog.py:13
pinocchio::Jexp6
void Jexp6(const MotionDense< MotionDerived > &nu, const Eigen::MatrixBase< Matrix6Like > &Jexp)
Derivative of exp6 Computed as the inverse of Jlog6.
Definition: spatial/explog.hpp:496
PINOCCHIO_EIGEN_CONST_CAST
#define PINOCCHIO_EIGEN_CONST_CAST(TYPE, OBJ)
Macro for an automatic const_cast.
Definition: eigen-macros.hpp:51
pinocchio::SE3Tpl< context::Scalar, context::Options >
inverse-kinematics.i
int i
Definition: inverse-kinematics.py:17
pinocchio::SE3Tpl< context::Scalar, context::Options >::Matrix6
traits< SE3Tpl >::Matrix6 Matrix6
Definition: spatial/se3-tpl.hpp:59
capsule-approximation.EPSILON
int EPSILON
Definition: capsule-approximation.py:20
PINOCCHIO_COMPILER_DIAGNOSTIC_POP
#define PINOCCHIO_COMPILER_DIAGNOSTIC_POP
Definition: include/pinocchio/macros.hpp:130
pinocchio::python::Scalar
context::Scalar Scalar
Definition: admm-solver.cpp:29
pinocchio::exp3
Eigen::Matrix< typename Vector3Like::Scalar, 3, 3, PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like)::Options > exp3(const Eigen::MatrixBase< Vector3Like > &v)
Exp: so3 -> SO3.
Definition: spatial/explog.hpp:36
explog.hpp
pinocchio::res
ReturnType res
Definition: spatial/classic-acceleration.hpp:57
pinocchio::Jlog6
void Jlog6(const SE3Tpl< Scalar, Options > &M, const Eigen::MatrixBase< Matrix6Like > &Jlog)
Derivative of log6.
Definition: spatial/explog.hpp:668
R
R
pinocchio::SE3Tpl::Interpolate
static SE3Tpl Interpolate(const SE3Tpl &A, const SE3Tpl &B, const OtherScalar &alpha)
Linear interpolation on the SE3 manifold.
PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED
#define PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED
Definition: include/pinocchio/macros.hpp:134
pinocchio::Jlog3
void Jlog3(const Scalar &theta, const Eigen::MatrixBase< Vector3Like > &log, const Eigen::MatrixBase< Matrix3Like > &Jlog)
Derivative of log3.
Definition: spatial/explog.hpp:240
pinocchio::SE3Tpl< context::Scalar, context::Options >::Matrix3
traits< SE3Tpl >::Matrix3 Matrix3
Definition: spatial/se3-tpl.hpp:56
BOOST_AUTO_TEST_CASE
BOOST_AUTO_TEST_CASE(exp)
Definition: explog.cpp:18
dcrba.C
C
Definition: dcrba.py:491
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::quaternion::exp3
void exp3(const Eigen::MatrixBase< Vector3Like > &v, Eigen::QuaternionBase< QuaternionLike > &quat_out)
Exp: so3 -> SO3 (quaternion)
Definition: explog-quaternion.hpp:27
pinocchio::exp6
SE3Tpl< typename MotionDerived::Scalar, PINOCCHIO_EIGEN_PLAIN_TYPE(typename MotionDerived::Vector3)::Options > exp6(const MotionDense< MotionDerived > &nu)
Exp: se3 -> SE3.
Definition: spatial/explog.hpp:347
D
D
pinocchio::SE3Tpl::rotation
ConstAngularRef rotation() const
Definition: se3-base.hpp:48
pinocchio::quaternion::log3
Eigen::Matrix< typename QuaternionLike::Scalar, 3, 1, PINOCCHIO_EIGEN_PLAIN_TYPE(typename QuaternionLike::Vector3)::Options > log3(const Eigen::QuaternionBase< QuaternionLike > &quat, typename QuaternionLike::Scalar &theta)
Same as log3 but with a unit quaternion as input.
Definition: explog-quaternion.hpp:211
pinocchio::q0
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorIn1 > & q0
Definition: joint-configuration.hpp:1137
pinocchio::inverse
void inverse(const Eigen::MatrixBase< MatrixIn > &m_in, const Eigen::MatrixBase< MatrixOut > &dest)
Definition: math/matrix.hpp:273
simulation-contact-dynamics.A
A
Definition: simulation-contact-dynamics.py:110
PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
#define PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
macros for pragma push/pop/ignore deprecated warnings
Definition: include/pinocchio/macros.hpp:129
M
M
pinocchio::context::Vector3
Eigen::Matrix< Scalar, 3, 1, Options > Vector3
Definition: context/generic.hpp:53
ur5x4.w
w
Definition: ur5x4.py:50
pinocchio::MotionTpl::Random
static MotionTpl Random()
Definition: motion-tpl.hpp:140
pinocchio::Motion
MotionTpl<::CppAD::AD< double >, 0 > Motion
Definition: context/cppad.hpp:37
Jexp3QuatLocal
void Jexp3QuatLocal(const Eigen::QuaternionBase< QuaternionLike > &quat, const Eigen::MatrixBase< Matrix43Like > &Jexp)
Definition: explog.cpp:254
pinocchio::q
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > & q
Definition: joint-configuration.hpp:1083
pinocchio::quaternion::Jlog3
void Jlog3(const Eigen::QuaternionBase< QuaternionLike > &quat, const Eigen::MatrixBase< Matrix3Like > &Jlog)
Computes the Jacobian of log3 operator for a unit quaternion.
Definition: explog-quaternion.hpp:332
pinocchio::v
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > const Eigen::MatrixBase< TangentVectorType > & v
Definition: joint-configuration.hpp:1084
pinocchio::SE3Tpl< context::Scalar, context::Options >::Quaternion
Eigen::Quaternion< Scalar, Options > Quaternion
Definition: spatial/se3-tpl.hpp:54
pinocchio::SE3Tpl::Random
static SE3Tpl Random()
Definition: spatial/se3-tpl.hpp:154
pinocchio::log6
MotionTpl< Scalar, Options > log6(const SE3Tpl< Scalar, Options > &M)
Log: SE3 -> se3.
Definition: spatial/explog.hpp:435
pinocchio::MotionTpl::Zero
static MotionTpl Zero()
Definition: motion-tpl.hpp:136
dcrba.dM
dM
Definition: dcrba.py:452
pinocchio::SE3Tpl::Identity
static SE3Tpl Identity()
Definition: spatial/se3-tpl.hpp:136
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
dcrba.eps
int eps
Definition: dcrba.py:458
pinocchio.explog.log
def log(x)
Definition: bindings/python/pinocchio/explog.py:29
pinocchio::Jexp3
void Jexp3(const Eigen::MatrixBase< Vector3Like > &r, const Eigen::MatrixBase< Matrix3Like > &Jexp)
Derivative of .
Definition: spatial/explog.hpp:118
pinocchio::log3
Eigen::Matrix< typename Matrix3Like::Scalar, 3, 1, PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix3Like)::Options > log3(const Eigen::MatrixBase< Matrix3Like > &R, typename Matrix3Like::Scalar &theta)
Same as log3.
Definition: spatial/explog.hpp:83
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::MotionTpl
Definition: context/casadi.hpp:27
pinocchio
Main pinocchio namespace.
Definition: timings.cpp:27
isApprox
bool isApprox(const Box &s1, const Box &s2, const FCL_REAL tol)

pinocchio
Author(s):
autogenerated on Thu Dec 19 2024 03:41:29