explog.cpp
Go to the documentation of this file.
1 //
2 // Copyright (c) 2016-2019 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 <boost/test/unit_test.hpp>
9 #include <boost/utility/binary.hpp>
10 
11 #include "utils/macros.hpp"
12 
13 using namespace pinocchio;
14 
15 BOOST_AUTO_TEST_SUITE ( BOOST_TEST_MODULE )
16 
17 BOOST_AUTO_TEST_CASE(exp)
18 {
19  SE3 M(SE3::Random());
20  Motion v(Motion::Random()); v.linear().setZero();
21 
22  SE3::Matrix3 R = exp3(v.angular());
23  BOOST_CHECK(R.isApprox(Eigen::AngleAxis<double>(v.angular().norm(), v.angular().normalized()).matrix()));
24 
25  SE3::Matrix3 R0 = exp3(SE3::Vector3::Zero());
26  BOOST_CHECK(R0.isIdentity());
27 
28  // check the NAN case
29  #ifdef NDEBUG
30  Motion::Vector3 vec3_nan(Motion::Vector3::Constant(NAN));
31  SE3::Matrix3 R_nan = exp3(vec3_nan);
32  BOOST_CHECK(R_nan != R_nan);
33  #endif
34 
35  M = exp6(v);
36 
37  BOOST_CHECK(R.isApprox(M.rotation()));
38 
39  // check the NAN case
40  #ifdef NDEBUG
41  Motion::Vector6 vec6_nan(Motion::Vector6::Constant(NAN));
42  SE3 M_nan = exp6(vec6_nan);
43  BOOST_CHECK(M_nan != M_nan);
44  #endif
45 
46  R = exp3(SE3::Vector3::Zero());
47  BOOST_CHECK(R.isIdentity());
48 
49  // Quaternion
50  Eigen::Quaterniond quat;
51  quaternion::exp3(v.angular(),quat);
52  BOOST_CHECK(quat.toRotationMatrix().isApprox(M.rotation()));
53 
54  quaternion::exp3(SE3::Vector3::Zero(),quat);
55  BOOST_CHECK(quat.toRotationMatrix().isIdentity());
56  BOOST_CHECK(quat.vec().isZero() && quat.coeffs().tail<1>().isOnes());
57 
58  // Check QuaternionMap
59  Eigen::Vector4d vec4;
60  Eigen::QuaternionMapd quat_map(vec4.data());
61  quaternion::exp3(v.angular(),quat_map);
62  BOOST_CHECK(quat_map.toRotationMatrix().isApprox(M.rotation()));
63 }
64 
65 BOOST_AUTO_TEST_CASE(log)
66 {
67  SE3 M(SE3::Identity());
68  Motion v(Motion::Random()); v.linear().setZero();
69 
70  SE3::Vector3 omega = log3(M.rotation());
71  BOOST_CHECK(omega.isZero());
72 
73  // check the NAN case
74 #ifdef NDEBUG
75  SE3::Matrix3 mat3_nan(SE3::Matrix3::Constant(NAN));
76  SE3::Vector3 omega_nan = log3(mat3_nan);
77  BOOST_CHECK(omega_nan != omega_nan);
78 #endif
79 
80  M.setRandom();
81  M.translation().setZero();
82 
83  v = log6(M);
84  omega = log3(M.rotation());
85  BOOST_CHECK(omega.isApprox(v.angular()));
86 
87  // check the NAN case
88  #ifdef NDEBUG
89  SE3::Matrix4 mat4_nan(SE3::Matrix4::Constant(NAN));
90  Motion::Vector6 v_nan = log6(mat4_nan);
91  BOOST_CHECK(v_nan != v_nan);
92  #endif
93 
94  // Quaternion
95  Eigen::Quaterniond quat(SE3::Matrix3::Identity());
96  omega = quaternion::log3(quat);
97  BOOST_CHECK(omega.isZero());
98 
99  for(int k = 0; k < 1e3; ++k)
100  {
101  SE3::Vector3 w; w.setRandom();
102  quaternion::exp3(w,quat);
103  SE3::Matrix3 rot = exp3(w);
104 
105  BOOST_CHECK(quat.toRotationMatrix().isApprox(rot));
106  double theta;
107  omega = quaternion::log3(quat,theta);
108  const double PI_value = PI<double>();
109  BOOST_CHECK(omega.norm() <= PI_value);
110  double theta_ref;
111  SE3::Vector3 omega_ref = log3(quat.toRotationMatrix(),theta_ref);
112 
113  BOOST_CHECK(omega.isApprox(omega_ref));
114  }
115 
116 
117  // Check QuaternionMap
118  Eigen::Vector4d vec4;
119  Eigen::QuaternionMapd quat_map(vec4.data());
120  quat_map = SE3::Random().rotation();
121  BOOST_CHECK(quaternion::log3(quat_map).isApprox(log3(quat_map.toRotationMatrix())));
122 }
123 
124 BOOST_AUTO_TEST_CASE(explog3)
125 {
126  SE3 M(SE3::Random());
127  SE3::Matrix3 M_res = exp3(log3(M.rotation()));
128  BOOST_CHECK(M_res.isApprox(M.rotation()));
129 
130  Motion::Vector3 v; v.setRandom();
131  Motion::Vector3 v_res = log3(exp3(v));
132  BOOST_CHECK(v_res.isApprox(v));
133 }
134 
135 BOOST_AUTO_TEST_CASE(explog3_quaternion)
136 {
137  SE3 M(SE3::Random());
138  Eigen::Quaterniond quat;
139  quat = M.rotation();
140  Eigen::Quaterniond quat_res;
141  quaternion::exp3(quaternion::log3(quat),quat_res);
142  BOOST_CHECK(quat_res.isApprox(quat) || quat_res.coeffs().isApprox(-quat.coeffs()));
143 
144  Motion::Vector3 v; v.setRandom();
145  quaternion::exp3(v,quat);
146  BOOST_CHECK(quaternion::log3(quat).isApprox(v));
147 
148  SE3::Matrix3 R_next = M.rotation() * exp3(v);
149  Motion::Vector3 v_est = log3(M.rotation().transpose() * R_next);
150  BOOST_CHECK(v_est.isApprox(v));
151 
152  SE3::Quaternion quat_v;
153  quaternion::exp3(v,quat_v);
154  SE3::Quaternion quat_next = quat * quat_v;
155  v_est = quaternion::log3(quat.conjugate() * quat_next);
156  BOOST_CHECK(v_est.isApprox(v));
157 }
158 
159 BOOST_AUTO_TEST_CASE(Jlog3_fd)
160 {
161  SE3 M(SE3::Random());
162  SE3::Matrix3 R (M.rotation());
163 
164  SE3::Matrix3 Jfd, Jlog;
165  Jlog3 (R, Jlog);
166  Jfd.setZero();
167 
168  Motion::Vector3 dR; dR.setZero();
169  const double eps = 1e-8;
170  for (int i = 0; i < 3; ++i)
171  {
172  dR[i] = eps;
173  SE3::Matrix3 R_dR = R * exp3(dR);
174  Jfd.col(i) = (log3(R_dR) - log3(R)) / eps;
175  dR[i] = 0;
176  }
177  BOOST_CHECK(Jfd.isApprox(Jlog, std::sqrt(eps)));
178 }
179 
180 BOOST_AUTO_TEST_CASE(Jexp3_fd)
181 {
182  SE3 M(SE3::Random());
183  SE3::Matrix3 R (M.rotation());
184 
185  Motion::Vector3 v = log3(R);
186 
187  SE3::Matrix3 Jexp_fd, Jexp;
188 
189  Jexp3(Motion::Vector3::Zero(), Jexp);
190  BOOST_CHECK(Jexp.isIdentity());
191 
192  Jexp3(v, Jexp);
193 
194  Motion::Vector3 dv; dv.setZero();
195  const double eps = 1e-8;
196  for (int i = 0; i < 3; ++i)
197  {
198  dv[i] = eps;
199  SE3::Matrix3 R_next = exp3(v+dv);
200  Jexp_fd.col(i) = log3(R.transpose()*R_next) / eps;
201  dv[i] = 0;
202  }
203  BOOST_CHECK(Jexp_fd.isApprox(Jexp, std::sqrt(eps)));
204 }
205 
206 template<typename QuaternionLike, typename Matrix43Like>
207 void Jexp3QuatLocal(const Eigen::QuaternionBase<QuaternionLike> & quat,
208  const Eigen::MatrixBase<Matrix43Like> & Jexp)
209 {
210  Matrix43Like & Jout = PINOCCHIO_EIGEN_CONST_CAST(Matrix43Like,Jexp);
211 
212  skew(0.5 * quat.vec(),Jout.template topRows<3>());
213  Jout.template topRows<3>().diagonal().array() += 0.5 * quat.w();
214  Jout.template bottomRows<1>() = -0.5 * quat.vec().transpose();
215 }
216 
217 BOOST_AUTO_TEST_CASE(Jexp3_quat_fd)
218 {
219  typedef double Scalar;
220  SE3::Vector3 w; w.setRandom();
221  SE3::Quaternion quat; quaternion::exp3(w,quat);
222 
223  typedef Eigen::Matrix<Scalar,4,3> Matrix43;
224  Matrix43 Jexp3, Jexp3_fd;
225  quaternion::Jexp3CoeffWise(w,Jexp3);
226  SE3::Vector3 dw; dw.setZero();
227  const double eps = 1e-8;
228 
229  for(int i = 0; i < 3; ++i)
230  {
231  dw[i] = eps;
232  SE3::Quaternion quat_plus; quaternion::exp3(w + dw,quat_plus);
233  Jexp3_fd.col(i) = (quat_plus.coeffs() - quat.coeffs()) / eps;
234  dw[i] = 0;
235  }
236  BOOST_CHECK(Jexp3.isApprox(Jexp3_fd,sqrt(eps)));
237 
238  SE3::Matrix3 Jlog;
239  pinocchio::Jlog3(quat.toRotationMatrix(),Jlog);
240 
241  Matrix43 Jexp_quat_local;
242  Jexp3QuatLocal(quat,Jexp_quat_local);
243 
244 
245  Matrix43 Jcompositon = Jexp3 * Jlog;
246  BOOST_CHECK(Jcompositon.isApprox(Jexp_quat_local));
247 // std::cout << "Jcompositon\n" << Jcompositon << std::endl;
248 // std::cout << "Jexp_quat_local\n" << Jexp_quat_local << std::endl;
249 
250  // Arount zero
251  w.setZero();
252  w.fill(1e-6);
253  quaternion::exp3(w,quat);
254  quaternion::Jexp3CoeffWise(w,Jexp3);
255  for(int i = 0; i < 3; ++i)
256  {
257  dw[i] = eps;
258  SE3::Quaternion quat_plus; quaternion::exp3(w + dw,quat_plus);
259  Jexp3_fd.col(i) = (quat_plus.coeffs() - quat.coeffs()) / eps;
260  dw[i] = 0;
261  }
262  BOOST_CHECK(Jexp3.isApprox(Jexp3_fd,sqrt(eps)));
263 
264 }
265 
266 BOOST_AUTO_TEST_CASE(Jexp3_quat)
267 {
268  SE3 M(SE3::Random());
269  SE3::Quaternion quat(M.rotation());
270 
271  Motion dv(Motion::Zero());
272  const double eps = 1e-8;
273 
274  typedef Eigen::Matrix<double,7,6> Matrix76;
275  Matrix76 Jexp6_fd, Jexp6_quat; Jexp6_quat.setZero();
276  typedef Eigen::Matrix<double,4,3> Matrix43;
277  Matrix43 Jexp3_quat; Jexp3QuatLocal(quat,Jexp3_quat);
278  SE3 M_next;
279 
280  Jexp6_quat.middleRows<3>(Motion::LINEAR).middleCols<3>(Motion::LINEAR) = M.rotation();
281  Jexp6_quat.middleRows<4>(Motion::ANGULAR).middleCols<3>(Motion::ANGULAR) = Jexp3_quat;// * Jlog6_SE3.middleRows<3>(Motion::ANGULAR);
282  for(int i = 0; i < 6; ++i)
283  {
284  dv.toVector()[i] = eps;
285  M_next = M * exp6(dv);
286  const SE3::Quaternion quat_next(M_next.rotation());
287  Jexp6_fd.middleRows<3>(Motion::LINEAR).col(i) = (M_next.translation() - M.translation())/eps;
288  Jexp6_fd.middleRows<4>(Motion::ANGULAR).col(i) = (quat_next.coeffs() - quat.coeffs())/eps;
289  dv.toVector()[i] = 0.;
290  }
291 
292  BOOST_CHECK(Jexp6_quat.isApprox(Jexp6_fd,sqrt(eps)));
293 }
294 
295 BOOST_AUTO_TEST_CASE(Jexplog3)
296 {
297  Motion v(Motion::Random());
298 
299  Eigen::Matrix3d R (exp3(v.angular())),
300  Jexp, Jlog;
301  Jexp3 (v.angular(), Jexp);
302  Jlog3 (R , Jlog);
303 
304  BOOST_CHECK((Jlog * Jexp).isIdentity());
305 
306  SE3 M(SE3::Random());
307  R = M.rotation();
308  v.angular() = log3(R);
309  Jlog3 (R , Jlog);
310  Jexp3 (v.angular(), Jexp);
311 
312  BOOST_CHECK((Jexp * Jlog).isIdentity());
313 }
314 
315 BOOST_AUTO_TEST_CASE(Jlog3_quat)
316 {
317  SE3::Vector3 w; w.setRandom();
318  SE3::Quaternion quat; quaternion::exp3(w,quat);
319 
320  SE3::Matrix3 R(quat.toRotationMatrix());
321 
322  SE3::Matrix3 res, res_ref;
323  quaternion::Jlog3(quat,res);
324  Jlog3(R,res_ref);
325 
326  BOOST_CHECK(res.isApprox(res_ref));
327 }
328 
329 BOOST_AUTO_TEST_CASE(explog6)
330 {
331  SE3 M(SE3::Random());
332  SE3 M_res = exp6(log6(M));
333  BOOST_CHECK(M_res.isApprox(M));
334 
335  Motion v(Motion::Random());
336  Motion v_res = log6(exp6(v));
337  BOOST_CHECK(v_res.toVector().isApprox(v.toVector()));
338 }
339 
340 BOOST_AUTO_TEST_CASE(Jlog6_fd)
341 {
342  SE3 M(SE3::Random());
343 
344  SE3::Matrix6 Jfd, Jlog;
345  Jlog6 (M, Jlog);
346  Jfd.setZero();
347 
348  Motion dM; dM.setZero();
349  double step = 1e-8;
350  for (int i = 0; i < 6; ++i)
351  {
352  dM.toVector()[i] = step;
353  SE3 M_dM = M * exp6(dM);
354  Jfd.col(i) = (log6(M_dM).toVector() - log6(M).toVector()) / step;
355  dM.toVector()[i] = 0;
356  }
357 
358  BOOST_CHECK(Jfd.isApprox(Jlog, sqrt(step)));
359 }
360 
361 BOOST_AUTO_TEST_CASE(Jlog6_of_product_fd)
362 {
363  SE3 Ma(SE3::Random());
364  SE3 Mb(SE3::Random());
365 
366  SE3::Matrix6 Jlog, Ja, Jb, Jfda, Jfdb;
367  Jlog6 (Ma.inverse() * Mb, Jlog);
368  Ja = - Jlog * (Ma.inverse() * Mb).toActionMatrixInverse();
369  Jb = Jlog;
370  Jfda.setZero();
371  Jfdb.setZero();
372 
373  Motion dM; dM.setZero();
374  double step = 1e-8;
375  for (int i = 0; i < 6; ++i)
376  {
377  dM.toVector()[i] = step;
378  Jfda.col(i) = (log6((Ma*exp6(dM)).inverse()*Mb).toVector() - log6(Ma.inverse()*Mb).toVector()) / step;
379  Jfdb.col(i) = (log6(Ma.inverse()*Mb*exp6(dM)).toVector() - log6(Ma.inverse()*Mb).toVector()) / step;
380  dM.toVector()[i] = 0;
381  }
382 
383  BOOST_CHECK(Jfda.isApprox(Ja, sqrt(step)));
384  BOOST_CHECK(Jfdb.isApprox(Jb, sqrt(step)));
385 }
386 
387 BOOST_AUTO_TEST_CASE(Jexplog6)
388 {
389  Motion v(Motion::Random());
390 
391  SE3 M (exp6(v));
392  {
393  Eigen::Matrix<double, 6, 6, Eigen::RowMajor> Jexp, Jlog;
394  Jexp6 (v, Jexp);
395  Jlog6 (M, Jlog);
396 
397  BOOST_CHECK((Jlog * Jexp).isIdentity());
398  }
399 
400  M.setRandom();
401 
402  v = log6(M);
403  {
404  Eigen::Matrix<double, 6, 6> Jexp, Jlog;
405  Jlog6 (M, Jlog);
406  Jexp6 (v, Jexp);
407 
408  BOOST_CHECK((Jexp * Jlog).isIdentity());
409  }
410 }
411 
412 BOOST_AUTO_TEST_CASE(Hlog3_fd)
413 {
414  typedef SE3::Vector3 Vector3;
415  typedef SE3::Matrix3 Matrix3;
416  SE3::Quaternion q; quaternion::uniformRandom(q);
417  Matrix3 R (q.matrix());
418 
419  // Hlog3(R, v) returns the matrix H * v
420  // We check that H * e_k matches the finite difference of Hlog3(R, e_k)
421  Vector3 dR; dR.setZero();
422  double step = 1e-8;
423  for (int k = 0; k < 3; ++k)
424  {
425  Vector3 e_k (Vector3::Zero());
426  e_k[k] = 1.;
427 
428  Matrix3 Hlog_e_k;
429  Hlog3(R, e_k, Hlog_e_k);
430 
431  Matrix3 R_dR = R * exp3(step * e_k);
432  Matrix3 Jlog_R, Jlog_R_dR;
433  Jlog3(R, Jlog_R);
434  Jlog3(R_dR, Jlog_R_dR);
435 
436  Matrix3 Hlog_e_k_fd = (Jlog_R_dR - Jlog_R ) / step;
437 
438  BOOST_CHECK(Hlog_e_k.isApprox(Hlog_e_k_fd, sqrt(step)));
439  }
440 }
441 
442 BOOST_AUTO_TEST_CASE (test_basic)
443 {
444  typedef pinocchio::SE3::Vector3 Vector3;
445  typedef pinocchio::SE3::Matrix3 Matrix3;
446  typedef Eigen::Matrix4d Matrix4;
447  typedef pinocchio::Motion::Vector6 Vector6;
448 
449  const double EPSILON = 1e-12;
450 
451  // exp3 and log3.
452  Vector3 v3(Vector3::Random());
453  Matrix3 R(pinocchio::exp3(v3));
454  BOOST_CHECK(R.transpose().isApprox(R.inverse(), EPSILON));
455  BOOST_CHECK_SMALL(R.determinant() - 1.0, EPSILON);
456  Vector3 v3FromLog(pinocchio::log3(R));
457  BOOST_CHECK(v3.isApprox(v3FromLog, EPSILON));
458 
459  // exp6 and log6.
460  pinocchio::Motion nu = pinocchio::Motion::Random();
461  pinocchio::SE3 m = pinocchio::exp6(nu);
462  BOOST_CHECK(m.rotation().transpose().isApprox(m.rotation().inverse(),
463  EPSILON));
464  BOOST_CHECK_SMALL(m.rotation().determinant() - 1.0, EPSILON);
465  pinocchio::Motion nuFromLog(pinocchio::log6(m));
466  BOOST_CHECK(nu.linear().isApprox(nuFromLog.linear(), EPSILON));
467  BOOST_CHECK(nu.angular().isApprox(nuFromLog.angular(), EPSILON));
468 
469  Vector6 v6(Vector6::Random());
470  pinocchio::SE3 m2(pinocchio::exp6(v6));
471  BOOST_CHECK(m2.rotation().transpose().isApprox(m2.rotation().inverse(),
472  EPSILON));
473  BOOST_CHECK_SMALL(m2.rotation().determinant() - 1.0, EPSILON);
474  Matrix4 M = m2.toHomogeneousMatrix();
475  pinocchio::Motion nu2FromLog(pinocchio::log6(M));
476  Vector6 v6FromLog(nu2FromLog.toVector());
477  BOOST_CHECK(v6.isApprox(v6FromLog, EPSILON));
478 }
479 
480 BOOST_AUTO_TEST_CASE(test_SE3_interpolate)
481 {
482  SE3 A = SE3::Random();
483  SE3 B = SE3::Random();
484 
485  SE3 A_bis = SE3::Interpolate(A,B,0.);
486  BOOST_CHECK(A_bis.isApprox(A));
487  SE3 B_bis = SE3::Interpolate(A,B,1.);
488  BOOST_CHECK(B_bis.isApprox(B));
489 
490  A_bis = SE3::Interpolate(A,A,1.);
491  BOOST_CHECK(A_bis.isApprox(A));
492 
493  B_bis = SE3::Interpolate(B,B,1.);
494  BOOST_CHECK(B_bis.isApprox(B));
495 
496  SE3 C = SE3::Interpolate(A,B,0.5);
497  SE3 D = SE3::Interpolate(B,A,0.5);
498  BOOST_CHECK(D.isApprox(C));
499 }
500 
501 BOOST_AUTO_TEST_SUITE_END()
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: src/spatial/explog.hpp:75
pinocchio::v
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > const Eigen::MatrixBase< TangentVectorType > & v
Definition: joint-configuration.hpp:746
BOOST_AUTO_TEST_CASE
BOOST_AUTO_TEST_CASE(exp)
Definition: explog.cpp:17
pinocchio::Jexp3
void Jexp3(const Eigen::MatrixBase< Vector3Like > &r, const Eigen::MatrixBase< Matrix3Like > &Jexp)
Derivative of .
Definition: src/spatial/explog.hpp:111
pinocchio::Jlog3
void Jlog3(const Scalar &theta, const Eigen::MatrixBase< Vector3Like > &log, const Eigen::MatrixBase< Matrix3Like > &Jlog)
Derivative of log3.
Definition: src/spatial/explog.hpp:223
dcrba.eps
int eps
Definition: dcrba.py:384
Jexp3QuatLocal
void Jexp3QuatLocal(const Eigen::QuaternionBase< QuaternionLike > &quat, const Eigen::MatrixBase< Matrix43Like > &Jexp)
Definition: explog.cpp:207
dcrba.dM
dM
Definition: dcrba.py:383
pinocchio::Jlog6
void Jlog6(const SE3Tpl< Scalar, Options > &M, const Eigen::MatrixBase< Matrix6Like > &Jlog)
Derivative of log6.
Definition: src/spatial/explog.hpp:594
pinocchio::log6
MotionTpl< Scalar, Options > log6(const SE3Tpl< Scalar, Options > &M)
Log: SE3 -> se3.
Definition: src/spatial/explog.hpp:403
pinocchio::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:195
pinocchio::Jexp6
void Jexp6(const MotionDense< MotionDerived > &nu, const Eigen::MatrixBase< Matrix6Like > &Jexp)
Derivative of exp6 Computed as the inverse of Jlog6.
Definition: src/spatial/explog.hpp:439
pinocchio::quaternion::exp3
void exp3(const Eigen::MatrixBase< Vector3Like > &v, Eigen::QuaternionBase< QuaternionLike > &quat_out)
Exp: so3 -> SO3 (quaternion)
Definition: explog-quaternion.hpp:26
pinocchio::MotionBase::linear
ConstLinearType linear() const
Definition: motion-base.hpp:22
pinocchio::q
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > & q
Definition: joint-configuration.hpp:746
PINOCCHIO_EIGEN_CONST_CAST
#define PINOCCHIO_EIGEN_CONST_CAST(TYPE, OBJ)
Macro for an automatic const_cast.
Definition: eigen-macros.hpp:43
pinocchio::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::MotionBase::angular
ConstAngularType angular() const
Definition: motion-base.hpp:21
D
D
pinocchio::python::Scalar
SE3::Scalar Scalar
Definition: conversions.cpp:13
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: src/spatial/explog.hpp:34
pinocchio::inverse
void inverse(const Eigen::MatrixBase< MatrixIn > &m_in, const Eigen::MatrixBase< MatrixOut > &dest)
Definition: math/matrix.hpp:213
pinocchio::SE3Tpl< double, 0 >::Quaternion
Eigen::Quaternion< Scalar, Options > Quaternion
Definition: spatial/se3-tpl.hpp:52
pinocchio::quaternion::log3
Eigen::Matrix< typename QuaternionLike::Scalar, 3, 1, PINOCCHIO_EIGEN_PLAIN_TYPE(typename QuaternionLike::Vector3)::Options > log3(const Eigen::QuaternionBase< QuaternionLike > &quat, typename QuaternionLike::Scalar &theta)
Same as log3 but with a unit quaternion as input.
Definition: explog-quaternion.hpp:84
isApprox
bool isApprox(const Shape &s1, const Shape &s2, const FCL_REAL tol)
pinocchio::MotionTpl::Zero
static MotionTpl Zero()
Definition: motion-tpl.hpp:91
pinocchio::SE3Tpl::inverse
SE3Tpl inverse() const
aXb = bXa.inverse()
Definition: spatial/se3-tpl.hpp:115
pinocchio::SE3Tpl< double, 0 >::Matrix6
traits< SE3Tpl >::Matrix6 Matrix6
Definition: spatial/se3-tpl.hpp:57
dcrba.C
C
Definition: dcrba.py:412
pinocchio::SE3Tpl< double, 0 >::Matrix4
traits< SE3Tpl >::Matrix4 Matrix4
Definition: spatial/se3-tpl.hpp:55
R
R
quat
quat
pinocchio::SE3Base::toHomogeneousMatrix
HomogeneousMatrixType toHomogeneousMatrix() const
Definition: se3-base.hpp:44
m
float m
pinocchio::SE3Tpl< double, 0 >::Vector3
traits< SE3Tpl >::Vector3 Vector3
Definition: spatial/se3-tpl.hpp:53
pinocchio::quaternion::uniformRandom
void uniformRandom(const Eigen::QuaternionBase< Derived > &q)
Uniformly random quaternion sphere.
Definition: math/quaternion.hpp:108
pinocchio::SE3Base::rotation
ConstAngularRef rotation() const
Definition: se3-base.hpp:37
pinocchio
Main pinocchio namespace.
Definition: timings.cpp:28
pinocchio::MotionBase::toVector
ToVectorConstReturnType toVector() const
Definition: motion-base.hpp:37
B
B
res
res
pinocchio::SE3Base::isApprox
bool isApprox(const Derived &other, const Scalar &prec=Eigen::NumTraits< Scalar >::dummy_precision()) const
Definition: se3-base.hpp:108
pinocchio::Hlog3
void Hlog3(const Scalar &theta, const Eigen::MatrixBase< Vector3Like1 > &log, const Eigen::MatrixBase< Vector3Like2 > &v, const Eigen::MatrixBase< Matrix3Like > &vt_Hlog)
Definition: src/spatial/explog.hpp:256
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::exp6
SE3Tpl< typename MotionDerived::Scalar, PINOCCHIO_EIGEN_PLAIN_TYPE(typename MotionDerived::Vector3)::Options > exp6(const MotionDense< MotionDerived > &nu)
Exp: se3 -> SE3.
Definition: src/spatial/explog.hpp:326
M
M
A
A
pinocchio::SE3Tpl< double, 0 >::Matrix3
traits< SE3Tpl >::Matrix3 Matrix3
Definition: spatial/se3-tpl.hpp:54
pinocchio::MotionTpl::Random
static MotionTpl Random()
Definition: motion-tpl.hpp:92
ur5x4.w
w
Definition: ur5x4.py:45
pinocchio::SE3Tpl< double, 0 >::Interpolate
static SE3Tpl Interpolate(const SE3Tpl &A, const SE3Tpl &B, const OtherScalar &alpha)
Linear interpolation on the SE3 manifold.
pinocchio::SE3Base::translation
ConstLinearRef translation() const
Definition: se3-base.hpp:38

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