Commit acb40c65 by Justin Carpentier Committed by GitHub

### Merge pull request #890 from jmirabel/fix

[Doc] Add a cheat sheet
parents a28ec541 77cf434f
 ... ... @@ -675,3 +675,9 @@ MATHJAX_RELPATH = MathJax/ # the tag USE_MATHJAX is set to YES. MATHJAX_FORMAT = SVG #--------------------------------------------------------------------------- # Aliases #--------------------------------------------------------------------------- ALIASES += "cheatsheet=\xrefitem cheatsheet \"Remarkable identity\" \"Cheat sheet\""
 ... ... @@ -439,6 +439,7 @@ namespace pinocchio * SE3(p1.matrix(), q1.derived().template head<3>())).toVector(); } /// \cheatsheet \f$\frac{\partial\ominus}{\partial q_1} {}^1X_0 = - \frac{\partial\ominus}{\partial q_0} \f$ template void dDifference_impl (const Eigen::MatrixBase & q0, const Eigen::MatrixBase & q1, ... ...
 ... ... @@ -10,15 +10,17 @@ namespace pinocchio { /** The rigid transform aMb can be seen in two ways: * * - given a point p expressed in frame B by its coordinate vector Bp, aMb * computes its coordinates in frame A by Ap = aMb Bp. * - aMb displaces a solid S centered at frame A into the solid centered in * B. In particular, the origin of A is displaced at the origin of B: $^aM_b * ^aA = ^aB$. * - given a point p expressed in frame B by its coordinate vector \f$^bp \f$, \f$^aM_b \f$ * computes its coordinates in frame A by \f$^ap = {}^aM_b {}^bp \f$. * - \f$^aM_b \f$ displaces a solid S centered at frame A into the solid centered in * B. In particular, the origin of A is displaced at the origin of B: * \f$^aM_b {}^aA = {}^aB \f$. * The rigid displacement is stored as a rotation matrix and translation vector by: * aMb (x) = aRb*x + aAB * where aAB is the vector from origin A to origin B expressed in coordinates A. * \f$^aM_b x = {}^aR_b x + {}^aAB \f$ * where \f$^aAB\f$ is the vector from origin A to origin B expressed in coordinates A. * * \cheatsheet \f${}^aM_c = {}^aM_b {}^bM_c \f$ */ template struct SE3Base ... ... @@ -41,12 +43,21 @@ namespace pinocchio } operator HomogeneousMatrixType() const { return toHomogeneousMatrix(); } /** * @brief The action matrix \f${}^aX_b \f$ of \f${}^aM_b \f$. * * \cheatsheet \f${}^a\nu_c = {}^aX_b {}^b\nu_c \f$ */ ActionMatrixType toActionMatrix() const { return derived().toActionMatrix_impl(); } operator ActionMatrixType() const { return toActionMatrix(); } /** * @brief The action matrix \f${}^bX_a \f$ of \f${}^aM_b \f$. * \sa toActionMatrix() */ ActionMatrixType toActionMatrixInverse() const { return derived().toActionMatrixInverse_impl(); ... ...
 ... ... @@ -16,6 +16,10 @@ BOOST_CHECK_MESSAGE((Va).isApprox(Vb, precision), \ "check " #Va ".isApprox(" #Vb ") failed " \ "[\n" << (Va).transpose() << "\n!=\n" << (Vb).transpose() << "\n]") #define EIGEN_MATRIX_IS_APPROX(Va, Vb, precision) \ BOOST_CHECK_MESSAGE((Va).isApprox(Vb, precision), \ "check " #Va ".isApprox(" #Vb ") failed " \ "[\n" << (Va) << "\n!=\n" << (Vb) << "\n]") using namespace pinocchio; ... ... @@ -28,7 +32,7 @@ void test_lie_group_methods (T & jmodel, typename T::JointDataDerived &) typedef double Scalar; const Scalar prec = Eigen::NumTraits::dummy_precision(); IFVERBOSE std::cout << "Testing Joint over " << jmodel.shortname() << std::endl; BOOST_TEST_MESSAGE ("Testing Joint over " << jmodel.shortname()); typedef typename T::ConfigVector_t ConfigVector_t; typedef typename T::TangentVector_t TangentVector_t; ... ... @@ -176,7 +180,7 @@ struct LieGroup_Jdifference{ typedef typename T::Scalar Scalar; T lg; IFVERBOSE std::cout << lg.name() << std::endl; BOOST_TEST_MESSAGE (lg.name()); ConfigVector_t q[2], q_dv[2]; q[0] = lg.random(); q[1] = lg.random(); ... ... @@ -190,7 +194,7 @@ struct LieGroup_Jdifference{ const Scalar eps = 1e-6; for (int k = 0; k < 2; ++k) { IFVERBOSE std::cout << "Checking J" << k << '\n' << J[k] << std::endl; BOOST_TEST_MESSAGE ("Checking J" << k << '\n' << J[k]); q_dv[0] = q[0]; q_dv[1] = q[1]; // Check J[k] ... ... @@ -207,6 +211,36 @@ struct LieGroup_Jdifference{ dv[i] = 0; } } specificTests(lg); } template void specificTests(const T ) const {} template void specificTests(const SpecialEuclideanOperationTpl<3,Scalar,Options>) const { typedef SE3Tpl SE3; typedef SpecialEuclideanOperationTpl<3,Scalar,Options> LG_t; typedef typename LG_t::ConfigVector_t ConfigVector_t; typedef typename LG_t::JacobianMatrix_t JacobianMatrix_t; LG_t lg; ConfigVector_t q[2]; q[0] = lg.random(); q[1] = lg.random(); JacobianMatrix_t J[2]; lg.template dDifference (q[0], q[1], J[0]); lg.template dDifference (q[0], q[1], J[1]); SE3 om0 (typename SE3::Quaternion (q[0].template tail<4>()).matrix(), q[0].template head<3>()), om1 (typename SE3::Quaternion (q[1].template tail<4>()).matrix(), q[1].template head<3>()), _1m2 (om1.actInv (om0)) ; EIGEN_MATRIX_IS_APPROX (J[1] * _1m2.toActionMatrix(), - J[0], 1e-8); } }; ... ... @@ -269,7 +303,7 @@ struct LieGroup_JintegrateJdifference{ typedef typename T::JacobianMatrix_t JacobianMatrix_t; T lg; IFVERBOSE std::cout << lg.name() << std::endl; BOOST_TEST_MESSAGE (lg.name()); ConfigVector_t qa, qb (lg.nq()); qa = lg.random(); TangentVector_t v (lg.nv()); ... ... @@ -304,7 +338,7 @@ struct LieGroup_JintegrateCoeffWise ConfigVector_t q = lg.random(); TangentVector_t dv(TangentVector_t::Zero(lg.nv())); // std::cout << "name: " << lg.name() << std::endl; BOOST_TEST_MESSAGE (lg.name()); typedef Eigen::Matrix JacobianCoeffs; JacobianCoeffs Jintegrate(JacobianCoeffs::Zero(lg.nq(),lg.nv())); lg.integrateCoeffWiseJacobian(q,Jintegrate); ... ... @@ -321,9 +355,7 @@ struct LieGroup_JintegrateCoeffWise dv[i] = 0; } BOOST_CHECK(Jintegrate.isApprox(Jintegrate_fd,sqrt(eps))); // std::cout << "Jintegrate\n" << Jintegrate << std::endl; // std::cout << "Jintegrate_fd\n" << Jintegrate_fd << std::endl; EIGEN_MATRIX_IS_APPROX(Jintegrate, Jintegrate_fd, sqrt(eps)); } }; ... ...
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!