Commit 49d7a522 authored by Joseph Mirabel's avatar Joseph Mirabel
Browse files

[Doc] Add remarkable identities for Jlog6

parent fcf0ea0a
Pipeline #13012 passed with stage
in 133 minutes and 16 seconds
......@@ -461,6 +461,12 @@ namespace pinocchio
* \f]
* and
* \f[ \beta(x)=\left(\frac{1}{x^2} - \frac{\sin x}{2x(1-\cos x)}\right) \f]
*
* \cheatsheet For \f$ M = log_6(a \times b) \f$,
* \f$ \frac{\partial M}{\partial a} = Jlog_6(M) \times X_b^{-1} \f$
* and \f$ \frac{\partial M}{\partial b} = Jlog_6(M) \f$. \n
* For \f$ M = log_6(a^{-1}) \f$,
* \f$ \frac{\partial M}{\partial a} = - Jlog_6(M) \times X_a \f$.
*/
template<typename Scalar, int Options, typename Matrix6Like>
void Jlog6(const SE3Tpl<Scalar, Options> & M,
......
......@@ -358,6 +358,32 @@ BOOST_AUTO_TEST_CASE(Jlog6_fd)
BOOST_CHECK(Jfd.isApprox(Jlog, sqrt(step)));
}
BOOST_AUTO_TEST_CASE(Jlog6_of_product_fd)
{
SE3 Ma(SE3::Random());
SE3 Mb(SE3::Random());
SE3::Matrix6 Jlog, Ja, Jb, Jfda, Jfdb;
Jlog6 (Ma.inverse() * Mb, Jlog);
Ja = - Jlog * (Ma.inverse() * Mb).toActionMatrixInverse();
Jb = Jlog;
Jfda.setZero();
Jfdb.setZero();
Motion dM; dM.setZero();
double step = 1e-8;
for (int i = 0; i < 6; ++i)
{
dM.toVector()[i] = step;
Jfda.col(i) = (log6((Ma*exp6(dM)).inverse()*Mb).toVector() - log6(Ma.inverse()*Mb).toVector()) / step;
Jfdb.col(i) = (log6(Ma.inverse()*Mb*exp6(dM)).toVector() - log6(Ma.inverse()*Mb).toVector()) / step;
dM.toVector()[i] = 0;
}
BOOST_CHECK(Jfda.isApprox(Ja, sqrt(step)));
BOOST_CHECK(Jfdb.isApprox(Jb, sqrt(step)));
}
BOOST_AUTO_TEST_CASE(Jexplog6)
{
Motion v(Motion::Random());
......
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