Commit 9e5512e7 authored by Joseph Mirabel's avatar Joseph Mirabel Committed by Joseph Mirabel
Browse files

Invert second quaternion rather than first quaternion when interpolating.

Add unit test for interpolate.

* Inverting the second quaternion fits with the Eigen SLERP implementation.
parent 47477707
......@@ -199,12 +199,13 @@ namespace hpp {
// theta is between 0 and M_PI/2.
// sin(alpha*theta)/sin(theta) in 0 should be computed differently.
if (fabs (angle) > 1e-6) {
const value_type sintheta_inv = 1 / sin (theta);
result.segment (index, 4) =
(sin ((1-u)*theta)/sin (theta)) * invertor * q1.segment (index, 4) +
(sin (u*theta)/sin (theta)) * q2.segment (index, 4);
(sin ((1-u)*theta) * sintheta_inv) * q1.segment (index, 4) +
invertor * (sin (u*theta) * sintheta_inv) * q2.segment (index, 4);
} else {
result.segment (index, 4) =
(1-u) * invertor * q1.segment (index, 4) + u * q2.segment (index, 4);
(1-u) * q1.segment (index, 4) + invertor * u * q2.segment (index, 4);
}
}
......
......@@ -30,11 +30,14 @@
#include <boost/test/output_test_stream.hpp>
using boost::test_tools::output_test_stream;
#include <Eigen/Geometry>
#include <hpp/util/debug.hh>
#include <hpp/model/configuration.hh>
#include <hpp/model/object-factory.hh>
using hpp::model::vector_t;
using hpp::model::vectorIn_t;
using hpp::model::Configuration_t;
using hpp::model::JointPtr_t;
using hpp::model::ObjectFactory;
......@@ -47,6 +50,9 @@ using hpp::model::ConfigurationPtr_t;
using hpp::model::size_type;
using hpp::model::value_type;
typedef Eigen::AngleAxis <value_type> AngleAxis_t;
typedef Eigen::Quaternion <value_type> Quaternion_t;
// Create a robot with various types of joints
DevicePtr_t createRobot ()
{
......@@ -103,6 +109,52 @@ void shootRandomConfig (const DevicePtr_t& robot, Configuration_t& config)
}
}
vector_t slerp (vectorIn_t v0, vectorIn_t v1, const value_type t) {
Quaternion_t q0(v0[0], v0[1], v0[2], v0[3]);
Quaternion_t q1(v1[0], v1[1], v1[2], v1[3]);
Quaternion_t q = q0.slerp (t, q1);
vector_t res(4);
res[0] = q.w(); res[1] = q.x(); res[2] = q.y(); res[3] = q.z();
return res;
}
AngleAxis_t aa (vectorIn_t v0, vectorIn_t v1) {
Quaternion_t q0(v0[0], v0[1], v0[2], v0[3]);
Quaternion_t q1(v1[0], v1[1], v1[2], v1[3]);
return AngleAxis_t (q0 * q1.conjugate ());
}
vector_t interpolation (DevicePtr_t robot, vectorIn_t q0, vectorIn_t q1, int n)
{
const size_type rso3 = 4;
bool print = false;
vector_t q2 (q0);
value_type u = 0;
value_type step = (value_type)1 / (value_type)(n + 2);
vector_t angle1 (n+2), angle2(n+2);
if (print) std::cout << "HPP : ";
for (int i = 0; i < n + 2; ++i) {
u = 0 + i * step;
hpp::model::interpolate (robot, q0, q1, u, q2);
AngleAxis_t aa1 (aa (q0.segment<4>(rso3),q2.segment<4>(rso3)));
angle1[i] = aa1.angle ();
if (print) std::cout << aa1.angle () << ", ";
}
if (print) std::cout << "\n";
if (print) std::cout << "Eigen: ";
for (int i = 0; i < n + 2; ++i) {
u = 0 + i * step;
vector_t eigen_slerp = slerp (q0.segment<4>(rso3), q1.segment<4>(rso3), u);
AngleAxis_t aa2 (aa (q0.segment<4>(rso3),eigen_slerp));
angle2[i] = aa2.angle ();
if (print) std::cout << aa2.angle () << ", ";
}
if (print) std::cout << "\n";
return angle1 - angle2;
}
BOOST_AUTO_TEST_CASE(difference_and_integrate)
{
DevicePtr_t robot = createRobot ();
......@@ -138,3 +190,44 @@ BOOST_AUTO_TEST_CASE(difference_and_integrate)
);
}
}
BOOST_AUTO_TEST_CASE(interpolate)
{
DevicePtr_t robot = createRobot ();
Configuration_t q0; q0.resize (robot->configSize ());
Configuration_t q1; q1.resize (robot->configSize ());
Configuration_t q2; q2.resize (robot->configSize ());
vector_t q1_minus_q0; q1_minus_q0.resize (robot->numberDof ());
const value_type eps_dist = robot->numberDof() * sqrt(Eigen::NumTraits<value_type>::epsilon());
value_type distance;
for (size_type i=0; i<10000; ++i) {
shootRandomConfig (robot, q0);
shootRandomConfig (robot, q1);
hpp::model::interpolate (robot, q0, q1, 0, q2);
distance = hpp::model::distance (robot, q0, q2);
BOOST_CHECK_MESSAGE (distance < eps_dist,
"\n(q0 + 0 * (q1 - q0)) is not equivalent to q0\n" <<
"q0=" << q0.transpose () << "\n" <<
"q1=" << q1.transpose () << "\n" <<
"q2=" << q2.transpose () << "\n" <<
"distance=" << distance
);
const size_type rso3 = 4;
vector_t errors = interpolation (robot, q0, q1, 4);
BOOST_CHECK_MESSAGE (errors.isZero (),
"The interpolation computed by HPP does not match the Eigen SLERP"
);
hpp::model::interpolate (robot, q0, q1, 1, q2);
distance = hpp::model::distance (robot, q1, q2);
BOOST_CHECK_MESSAGE (distance < eps_dist,
"\n(q0 + 1 * (q1 - q0)) is not equivalent to q1\n" <<
"q0=" << q0.transpose () << "\n" <<
"q1=" << q1.transpose () << "\n" <<
"q2=" << q2.transpose () << "\n" <<
"distance=" << distance
);
}
}
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