Verified Commit dfbf8317 authored by Justin Carpentier's avatar Justin Carpentier
Browse files

spatial/explog: fix some Taylor expansions

follow discussions in PR #584
parent eb8d4a2d
......@@ -108,7 +108,9 @@ namespace se3
// From runs of hpp-constraints/tests/logarithm.cc: 1e-6 is too small.
if (theta < PI_value - 1e-2)
{
const Scalar t = ((theta > TaylorSeriesExpansion<Scalar>::template precision<4>())? theta / sin(theta) : Scalar(1)) / Scalar(2);
const Scalar t = ((theta > TaylorSeriesExpansion<Scalar>::template precision<4>())
? theta / sin(theta)
: Scalar(1)) / Scalar(2);
res(0) = t * (R (2, 1) - R (1, 2));
res(1) = t * (R (0, 2) - R (2, 0));
res(2) = t * (R (1, 0) - R (0, 1));
......@@ -171,10 +173,9 @@ namespace se3
if (n < TaylorSeriesExpansion<Scalar>::template precision<4>())
{
a = Scalar(1) - n/Scalar(6) + n2/Scalar(120);
b = - Scalar(1)/Scalar(2) + n/Scalar(24) - n2/Scalar(720);
c = Scalar(1)/Scalar(6) - n/Scalar(120) + n2/Scalar(5040);
a = Scalar(1) - n2/Scalar(6);
b = - Scalar(1)/Scalar(2) - n2/Scalar(24);
c = Scalar(1)/Scalar(6) - n2/Scalar(120);
}
else
{
......@@ -205,27 +206,30 @@ namespace se3
PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE (Matrix3Like, Jlog, 3, 3);
Matrix3Like & Jout = const_cast<Matrix3Like &>(Jlog.derived());
typedef typename Matrix3Like::Scalar Scalar3;
if (theta < TaylorSeriesExpansion<Scalar>::template precision<4>())
Jout.setIdentity();
{
const Scalar alpha = Scalar(1)/Scalar(12) + theta*theta / Scalar(720);
Jout.noalias() = alpha * log * log.transpose();
Jout.diagonal().array() += Scalar(0.5) * (2 - theta*theta / Scalar(6));
// Jlog += r_{\times}/2
addSkew(0.5 * log, Jlog);
}
else
{
// Jlog = alpha I
Scalar ct,st; SINCOS(theta,&st,&ct);
const Scalar st_1mct = st/(Scalar(1)-ct);
const Scalar alpha = Scalar(1)/(theta*theta) - st_1mct/(Scalar(2)*theta);
Jout.noalias() = alpha * log * log.transpose();
Jout.setZero();
Jout.diagonal().setConstant(theta*st_1mct);
Jout.diagonal().array() += Scalar(0.5) * (theta*st_1mct);
// Jlog += r_{\times}/2
Jout(0,1) = -log(2); Jout(1,0) = log(2);
Jout(0,2) = log(1); Jout(2,0) = -log(1);
Jout(1,2) = -log(0); Jout(2,1) = log(0);
Jout /= Scalar3(2);
const Scalar alpha = Scalar(1)/(theta*theta) - st_1mct/(Scalar(2)*theta);
Jout += alpha * log * log.transpose();
addSkew(0.5 * log, Jlog);
}
}
......
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