Skip to content
GitLab
Commit 47477707 authored by Joseph Mirabel's avatar Joseph Mirabel Committed by Joseph Mirabel
Browse files

Fix quaternion distance and add JointConfiguration::squaredDistance

parent cf2cdece
Hide whitespace changes
Inline Side-by-side
include/hpp/model/configuration.hh
View file @ 47477707
......@@ -105,6 +105,26 @@ namespace hpp {
result.tail (dim) = q2.tail (dim) - q1.tail (dim);
}
/// Distance between two configuration.
///
/// \param robot robot that describes the kinematic chain
/// \param q1 first configuration,
/// \param q2 second configuration,
inline value_type distance (const DevicePtr_t& robot, ConfigurationIn_t q1,
ConfigurationIn_t q2)
{
value_type result = 0;
const JointVector_t& jv (robot->getJointVector ());
for (model::JointVector_t::const_iterator itJoint = jv.begin ();
itJoint != jv.end (); itJoint++) {
size_type iC = (*itJoint)->rankInConfiguration ();
result += (*itJoint)->configuration ()->squaredDistance (q1, q2, iC);
}
const size_type& dim = robot->extraConfigSpace().dimension();
result += (q2.tail (dim) - q1.tail (dim)).squaredNorm ();
return sqrt (result);
}
/// Normalize configuration
///
/// Configuration space is a represented by a sub-manifold of a vector
......
include/hpp/model/joint-configuration.hh
View file @ 47477707
......@@ -66,6 +66,14 @@ namespace hpp {
const size_type& index,
ConfigurationOut_t result) = 0;
/// Squared distance between two configurations of the joint
/// \param q1, q2 two configurations of the robot
/// \param index index of first component of q1 and q2 corresponding to
/// the joint.
virtual value_type squaredDistance (ConfigurationIn_t q1,
ConfigurationIn_t q2,
const size_type& index) const = 0;
/// Distance between two configurations of the joint
/// \param q1, q2 two configurations of the robot
/// \param index index of first component of q1 and q2 corresponding to
......@@ -160,6 +168,9 @@ namespace hpp {
virtual value_type distance (ConfigurationIn_t q1,
ConfigurationIn_t q2,
const size_type& index) const;
value_type squaredDistance (ConfigurationIn_t q1,
ConfigurationIn_t q2,
const size_type& index) const;
virtual void integrate (ConfigurationIn_t q,
vectorIn_t v,
......@@ -198,6 +209,9 @@ namespace hpp {
virtual value_type distance (ConfigurationIn_t q1,
ConfigurationIn_t q2,
const size_type& index) const;
value_type squaredDistance (ConfigurationIn_t q1,
ConfigurationIn_t q2,
const size_type& index) const;
virtual void integrate (ConfigurationIn_t q,
vectorIn_t v,
const size_type& indexConfig,
......@@ -311,6 +325,9 @@ namespace hpp {
ConfigurationOut_t result);
value_type distance (ConfigurationIn_t q1, ConfigurationIn_t q2,
const size_type& index) const;
value_type squaredDistance (ConfigurationIn_t q1,
ConfigurationIn_t q2,
const size_type& index) const;
void integrate (ConfigurationIn_t q, vectorIn_t v,
const size_type& indexConfig,
const size_type& indexVelocity,
......@@ -338,6 +355,9 @@ namespace hpp {
ConfigurationOut_t result);
value_type distance (ConfigurationIn_t q1, ConfigurationIn_t q2,
const size_type& index) const;
value_type squaredDistance (ConfigurationIn_t q1,
ConfigurationIn_t q2,
const size_type& index) const;
void integrate (ConfigurationIn_t q, vectorIn_t v,
const size_type& indexConfig,
const size_type& indexVelocity,
......@@ -375,6 +395,9 @@ namespace hpp {
virtual value_type distance (ConfigurationIn_t q1,
ConfigurationIn_t q2,
const size_type& index) const;
value_type squaredDistance (ConfigurationIn_t q1,
ConfigurationIn_t q2,
const size_type& index) const;
virtual void integrate (ConfigurationIn_t q,
vectorIn_t v,
......
src/joint-configuration.cc
View file @ 47477707
......@@ -117,6 +117,13 @@ namespace hpp {
{
}
value_type AnchorJointConfig::squaredDistance (ConfigurationIn_t,
ConfigurationIn_t,
const size_type&) const
{
return 0;
}
value_type AnchorJointConfig::distance (ConfigurationIn_t,
ConfigurationIn_t,
const size_type&) const
......@@ -152,6 +159,8 @@ namespace hpp {
}
/// Compute quaternion and angle from a SO(3) joint configuration
/// \note this angle corresponds to the angle between q1 and q2, in \f$ \mathbb{R}^4 \f$.
/// The angle of the rotation going from q1 to q2 is half this angle.
///
/// \param q1, q2, robot configurations
/// \param index index of joint configuration in robot configuration vector
......@@ -199,6 +208,14 @@ namespace hpp {
}
}
value_type SO3JointConfig::squaredDistance (ConfigurationIn_t q1,
ConfigurationIn_t q2,
const size_type& index) const
{
const value_type d (distance (q1, q2, index));
return d*d;
}
value_type SO3JointConfig::distance (ConfigurationIn_t q1,
ConfigurationIn_t q2,
const size_type& index) const
......@@ -207,7 +224,7 @@ namespace hpp {
value_type theta = angleBetweenQuaternions (q1, q2, index, cosIsNegative);
assert (theta >= 0);
assert (M_PI - theta >= 0);
return (cosIsNegative) ? M_PI - theta : theta;
return 2 * (cosIsNegative ? M_PI - theta : theta);
}
void SO3JointConfig::integrate (ConfigurationIn_t q,
......@@ -302,6 +319,13 @@ namespace hpp {
u * q2.segment <dimension> (index);
}
template <size_type dimension>
value_type TranslationJointConfig <dimension>::squaredDistance
(ConfigurationIn_t q1, ConfigurationIn_t q2, const size_type& index) const
{
return (q2.segment <dimension> (index) - q1.segment <dimension> (index)).squaredNorm ();
}
template <size_type dimension>
value_type TranslationJointConfig <dimension>::distance
(ConfigurationIn_t q1, ConfigurationIn_t q2, const size_type& index) const
......@@ -401,6 +425,14 @@ namespace hpp {
}
}
value_type UnBounded::squaredDistance (ConfigurationIn_t q1,
ConfigurationIn_t q2,
const size_type& index) const
{
const value_type d (distance (q1, q2, index));
return d*d;
}
value_type UnBounded::distance (ConfigurationIn_t q1,
ConfigurationIn_t q2,
const size_type& index) const
......@@ -475,6 +507,14 @@ namespace hpp {
result [index] = (1-u) * q1 [index] + u * q2 [index];
}
value_type Bounded::squaredDistance (ConfigurationIn_t q1,
ConfigurationIn_t q2,
const size_type& index) const
{
// linearly interpolate
return (q2.segment<1>(index) - q1.segment<1>(index)).squaredNorm ();
}
value_type Bounded::distance (ConfigurationIn_t q1, ConfigurationIn_t q2,
const size_type& index) const
{
......
tests/test-configuration.cc
View file @ 47477707
......@@ -71,6 +71,7 @@ DevicePtr_t createRobot ()
// SO3 joint
position.setIdentity ();
JointPtr_t j1 = factory.createJointSO3 (position);
j1->name ("so3");
joint->addChildJoint (j1);
return robot;
......@@ -109,14 +110,31 @@ BOOST_AUTO_TEST_CASE(difference_and_integrate)
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());
for (size_type i=0; i<10000; ++i) {
shootRandomConfig (robot, q0);
shootRandomConfig (robot, q1);
hpp::model::difference (robot, q1, q0, q1_minus_q0);
hpp::model::integrate (robot, q0, q1_minus_q0, q2);
std::cout << "q1=" << q1.transpose () << std::endl;
std::cout << "q2=" << q2.transpose () << std::endl;
std::cout << "(q2 - q1).norm () = " << (q2 - q1).norm () << std::endl;
BOOST_CHECK ((q2 - q1).norm () < 1e-10);
// Check that the distance is the norm of the difference
value_type distance = hpp::model::distance (robot, q0, q1);
BOOST_CHECK_MESSAGE (distance - q1_minus_q0.norm () < Eigen::NumTraits<value_type>::dummy_precision(),
"\nThe distance is not the norm of the difference\n" <<
"q0=" << q0.transpose () << "\n" <<
"q1=" << q1.transpose () << "\n" <<
"distance=" << distance << "\n" <<
"(q1 - q0).norm () = " << q1_minus_q0.norm ()
);
// Check that distance (q0 + (q1 - q0), q1) is zero
distance = hpp::model::distance (robot, q1, q2);
BOOST_CHECK_MESSAGE (distance < eps_dist,
"\n(q0 + (q1 - q0)) is not equivalent to q1\n" <<
"q1=" << q1.transpose () << "\n" <<
"q2=" << q2.transpose () << "\n" <<
"distance=" << distance
);
}
}
Supports Markdown
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