Commit e14b5c78 authored by Florent Lamiraux's avatar Florent Lamiraux
Browse files

Add method isApprox to test that 2 configurations are numerically close.

parent e390326a
......@@ -105,6 +105,29 @@ namespace hpp {
result.tail (dim) = q1.tail (dim) - q2.tail (dim);
}
/// Test that two configurations are close
///
/// \param robot robot that describes the kinematic chain
/// \param q1 first configuration,
/// \param q2 second configuration,
/// \param eps numerical threshold
/// \return true if the configurations are closer than the numerical
/// threshold
bool inline isApprox (const DevicePtr_t& robot, ConfigurationIn_t q1,
ConfigurationIn_t q2, value_type eps)
{
const JointVector_t& jv (robot->getJointVector ());
for (model::JointVector_t::const_iterator itJoint = jv.begin ();
itJoint != jv.end (); itJoint++) {
size_type indexConfig = (*itJoint)->rankInConfiguration ();
if (!(*itJoint)->configuration ()->isApprox (q1, q2, indexConfig, eps))
return false;
}
const size_type& dim = robot->extraConfigSpace().dimension();
if (!q2.tail (dim).isApprox (q1.tail (dim), eps)) return false;
return true;
}
/// Distance between two configuration.
///
/// \param robot robot that describes the kinematic chain
......
......@@ -116,6 +116,19 @@ namespace hpp {
const size_type& indexVelocity,
vectorOut_t result) const = 0;
/// Test that two configurations are close
///
/// \param q1 first configuration,
/// \param q2 second configuration,
/// \param indexConfig index of first component of q corresponding to
/// the joint.
/// \param eps numerical threshold
/// \return true if the configurations are closer than the numerical
/// threshold
virtual bool isApprox (ConfigurationIn_t q1, ConfigurationIn_t q2,
const size_type& indexConfig,
const value_type& eps) const = 0;
/// Normalize configuration of joint
virtual void normalize (const size_type& index,
ConfigurationOut_t result) const = 0;
......@@ -182,6 +195,15 @@ namespace hpp {
const size_type& indexConfig,
const size_type& indexVelocity,
vectorOut_t result) const;
/// \param q2 second configuration,
/// \param indexConfig index of first component of q corresponding to
/// the joint.
/// \param eps numerical threshold
/// \return true if the configurations are closer than the numerical
/// threshold
virtual bool isApprox (ConfigurationIn_t q1, ConfigurationIn_t q2,
const size_type& indexConfig,
const value_type& eps) const;
/// Normalize configuration of joint
virtual void normalize (const size_type& index,
ConfigurationOut_t result) const;
......@@ -239,6 +261,15 @@ namespace hpp {
const size_type& indexConfig,
const size_type& indexVelocity,
vectorOut_t result) const;
/// \param q2 second configuration,
/// \param indexConfig index of first component of q corresponding to
/// the joint.
/// \param eps numerical threshold
/// \return true if the configurations are closer than the numerical
/// threshold
virtual bool isApprox (ConfigurationIn_t q1, ConfigurationIn_t q2,
const size_type& indexConfig,
const value_type& eps) const;
/// Normalize configuration of joint
virtual void normalize (const size_type& index,
ConfigurationOut_t result) const;
......@@ -305,6 +336,15 @@ namespace hpp {
const size_type& indexVelocity,
vectorOut_t result) const = 0;
/// \param q2 second configuration,
/// \param indexConfig index of first component of q corresponding to
/// the joint.
/// \param eps numerical threshold
/// \return true if the configurations are closer than the numerical
/// threshold
virtual bool isApprox (ConfigurationIn_t q1, ConfigurationIn_t q2,
const size_type& indexConfig,
const value_type& eps) const;
/// Normalize configuration of joint
virtual void normalize (const size_type& index,
ConfigurationOut_t result) const;
......@@ -336,6 +376,15 @@ namespace hpp {
const size_type& indexConfig,
const size_type& indexVelocity,
vectorOut_t result) const;
/// \param q2 second configuration,
/// \param indexConfig index of first component of q corresponding to
/// the joint.
/// \param eps numerical threshold
/// \return true if the configurations are closer than the numerical
/// threshold
virtual bool isApprox (ConfigurationIn_t q1, ConfigurationIn_t q2,
const size_type& indexConfig,
const value_type& eps) const;
/// Normalize configuration of joint
virtual void normalize (const size_type& index,
ConfigurationOut_t result) const;
......@@ -366,6 +415,15 @@ namespace hpp {
const size_type& indexConfig,
const size_type& indexVelocity,
vectorOut_t result) const;
/// \param q2 second configuration,
/// \param indexConfig index of first component of q corresponding to
/// the joint.
/// \param eps numerical threshold
/// \return true if the configurations are closer than the numerical
/// threshold
virtual bool isApprox (ConfigurationIn_t q1, ConfigurationIn_t q2,
const size_type& indexConfig,
const value_type& eps) const;
/// Normalize configuration of joint
virtual void normalize (const size_type& index,
ConfigurationOut_t result) const;
......@@ -425,6 +483,15 @@ namespace hpp {
const size_type& indexVelocity,
vectorOut_t result) const;
/// \param q2 second configuration,
/// \param indexConfig index of first component of q corresponding to
/// the joint.
/// \param eps numerical threshold
/// \return true if the configurations are closer than the numerical
/// threshold
virtual bool isApprox (ConfigurationIn_t q1, ConfigurationIn_t q2,
const size_type& indexConfig,
const value_type& eps) const;
/// Normalize configuration of joint
virtual void normalize (const size_type& index,
ConfigurationOut_t result) const;
......
......@@ -147,6 +147,14 @@ namespace hpp {
{
}
bool AnchorJointConfig::isApprox (ConfigurationIn_t q1,
ConfigurationIn_t q2,
const size_type& indexConfig,
const value_type& eps) const
{
return true;
}
/// Normalize configuration of joint
void AnchorJointConfig::normalize (const size_type&,
ConfigurationOut_t) const
......@@ -295,6 +303,16 @@ namespace hpp {
result.segment <3> (indexVelocity) = angleAxis.angle () * angleAxis.axis ();
}
bool SO3JointConfig::isApprox (ConfigurationIn_t q1, ConfigurationIn_t q2,
const size_type& indexConfig,
const value_type& eps) const
{
return (q1.segment <4> (indexConfig).isApprox
(q2.segment <4> (indexConfig)) ||
q1.segment <4> (indexConfig).isApprox
(-q2.segment <4> (indexConfig)));
}
/// Normalize configuration of joint
void SO3JointConfig::normalize (const size_type& index,
ConfigurationOut_t result) const
......@@ -373,6 +391,15 @@ namespace hpp {
q2.segment <dimension> (indexConfig);
}
template <size_type dimension>
bool TranslationJointConfig <dimension>::isApprox
(ConfigurationIn_t q1, ConfigurationIn_t q2,
const size_type& indexConfig, const value_type& eps) const
{
return q1.segment <dimension> (indexConfig).isApprox
(q2.segment <dimension> (indexConfig));
}
/// Normalize configuration of joint
/// Do nothing
template <size_type dimension>
......@@ -481,6 +508,15 @@ namespace hpp {
result [indexVelocity] = atan2 (s1*c2 - s2*c1, c1*c2 + s1*s2);
}
bool UnBounded::isApprox (ConfigurationIn_t q1,
ConfigurationIn_t q2,
const size_type& indexConfig,
const value_type& eps) const
{
return q1.segment <2> (indexConfig).isApprox
(q2.segment <2> (indexConfig));
}
/// Normalize configuration of joint
void UnBounded::normalize (const size_type& index,
ConfigurationOut_t result) const
......@@ -549,6 +585,23 @@ namespace hpp {
result [indexVelocity] = q1 [indexConfig] - q2 [indexConfig];
}
/// Test that two configurations are close
///
/// \param q1 first configuration,
/// \param q2 second configuration,
/// \param indexConfig index of first component of q corresponding to
/// the joint.
/// \param eps numerical threshold
/// \return true if the configurations are closer than the numerical
/// threshold
bool Bounded::isApprox (ConfigurationIn_t q1,
ConfigurationIn_t q2,
const size_type& indexConfig,
const value_type& eps) const
{
return (fabs (q1 [indexConfig] - q2 [indexConfig]) < eps);
}
/// Normalize configuration of joint
/// Do nothing.
void Bounded::normalize (const size_type&,
......
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