Unverified Commit acb40c65 authored by Justin Carpentier's avatar 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 <ArgumentPosition arg, class ConfigL_t, class ConfigR_t, class JacobianOut_t>
void dDifference_impl (const Eigen::MatrixBase<ConfigL_t> & q0,
const Eigen::MatrixBase<ConfigR_t> & 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<class Derived>
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<Scalar>::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 <typename T>
void specificTests(const T ) const
{}
template <typename Scalar, int Options>
void specificTests(const SpecialEuclideanOperationTpl<3,Scalar,Options>) const
{
typedef SE3Tpl<Scalar> 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<ARG0> (q[0], q[1], J[0]);
lg.template dDifference<ARG1> (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<Scalar,T::NQ,T::NV> 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!
Please register or to comment