Commit dcf6e5af authored by Joseph Mirabel's avatar Joseph Mirabel Committed by Joseph Mirabel
Browse files

Rename isValidConfiguration into isNormalized

parent 5f97bbb8
......@@ -134,8 +134,8 @@ namespace hpp {
/// Check if a configuration is normalized
///
/// It consists in checking that norm of quaternions and complex is one.
bool isValidConfiguration (const DevicePtr_t& robot, ConfigurationIn_t q,
const value_type& eps);
bool isNormalized (const DevicePtr_t& robot, ConfigurationIn_t q,
const value_type& eps);
/// Write configuration in a string
inline std::string displayConfig (ConfigurationIn_t q)
......
......@@ -94,11 +94,11 @@ namespace hpp {
}
template <class ConfigIn_t>
static bool isValidConfig(const Eigen::MatrixBase<ConfigIn_t > & q, const value_type& eps)
static bool isNormalized(const Eigen::MatrixBase<ConfigIn_t > & q, const value_type& eps)
{
EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigIn_t , Base::ConfigVector_t);
return LieGroup1::isValidConfig(q.template head<LieGroup1::NQ>(), eps)
&& LieGroup2::isValidConfig(q.template tail<LieGroup2::NQ>(), eps);
return LieGroup1::isNormalized(q.template head<LieGroup1::NQ>(), eps)
&& LieGroup2::isNormalized(q.template tail<LieGroup2::NQ>(), eps);
}
}; // struct CartesianProductOperation
} // namespace liegroup
......
......@@ -71,7 +71,7 @@ namespace hpp {
}
template <class ConfigIn_t>
static bool isValidConfig(const Eigen::MatrixBase<ConfigIn_t > & q, const value_type& eps)
static bool isNormalized(const Eigen::MatrixBase<ConfigIn_t > & q, const value_type& eps)
{
EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigIn_t , Base::ConfigVector_t);
return (std::abs(q.norm() - 1) < eps );
......
......@@ -90,7 +90,7 @@ namespace hpp {
}
template <class ConfigIn_t>
static bool isValidConfig(const Eigen::MatrixBase<ConfigIn_t > &, const value_type&)
static bool isNormalized(const Eigen::MatrixBase<ConfigIn_t > &, const value_type&)
{
return true;
}
......
......@@ -141,13 +141,13 @@ namespace hpp {
se3::normalize(robot->model(), q);
}
struct IsValidStep : public se3::fusion::JointModelVisitor<IsValidStep>
struct IsNormalizedStep : public se3::fusion::JointModelVisitor<IsNormalizedStep>
{
typedef boost::fusion::vector<ConfigurationIn_t,
const value_type &,
bool &> ArgsType;
JOINT_MODEL_VISITOR_INIT(IsValidStep);
JOINT_MODEL_VISITOR_INIT(IsNormalizedStep);
template<typename JointModel>
static void algo(const se3::JointModelBase<JointModel> & jmodel,
......@@ -156,26 +156,26 @@ namespace hpp {
bool & ret)
{
typedef typename LieGroupTpl::operation<JointModel>::type LG_t;
ret = ret && LG_t::isValidConfig(jmodel.jointConfigSelector(q), eps);
ret = ret && LG_t::isNormalized(jmodel.jointConfigSelector(q), eps);
}
};
template<>
void IsValidStep::algo<se3::JointModelComposite>(const se3::JointModelBase<se3::JointModelComposite> & jmodel,
void IsNormalizedStep::algo<se3::JointModelComposite>(const se3::JointModelBase<se3::JointModelComposite> & jmodel,
ConfigurationIn_t q,
const value_type & eps,
bool & ret)
{
se3::details::Dispatch<IsValidStep>::run(jmodel, IsValidStep::ArgsType(q, eps, ret));
se3::details::Dispatch<IsNormalizedStep>::run(jmodel, IsNormalizedStep::ArgsType(q, eps, ret));
}
bool isValidConfiguration (const DevicePtr_t& robot, ConfigurationIn_t q, const value_type& eps)
bool isNormalized (const DevicePtr_t& robot, ConfigurationIn_t q, const value_type& eps)
{
bool ret = true;
const se3::Model& model = robot->model();
for (std::size_t i = 1; i < model.njoints; ++i) {
IsValidStep::run(model.joints[i],
IsValidStep::ArgsType(q, eps, ret));
for (std::size_t i = 1; i < (std::size_t)model.njoints; ++i) {
IsNormalizedStep::run(model.joints[i],
IsNormalizedStep::ArgsType(q, eps, ret));
if (!ret) return false;
}
return true;
......
......@@ -119,25 +119,25 @@ BOOST_AUTO_TEST_CASE(is_valid_configuration)
robot = unittest::makeDevice(unittest::HumanoidRomeo);
Configuration_t q = robot->neutralConfiguration();
BOOST_CHECK(isValidConfiguration(robot, q, eps));
BOOST_CHECK(isNormalized(robot, q, eps));
/// Set a quaternion of norm != 1
q[3] = 1; q[4] = 1;
BOOST_CHECK(!isValidConfiguration(robot, q, eps));
BOOST_CHECK(!isNormalized(robot, q, eps));
robot = unittest::makeDevice(unittest::CarLike);
q = robot->neutralConfiguration();
BOOST_CHECK(isValidConfiguration(robot, q, eps));
BOOST_CHECK(isNormalized(robot, q, eps));
/// Set a quaternion of norm != 1
q[2] = 1; q[3] = 1;
BOOST_CHECK(!isValidConfiguration(robot, q, eps));
BOOST_CHECK(!isNormalized(robot, q, eps));
robot = unittest::makeDevice(unittest::CarLike);
q = robot->neutralConfiguration();
BOOST_CHECK(isValidConfiguration(robot, q, eps));
BOOST_CHECK(isNormalized(robot, q, eps));
}
void test_difference_and_distance(DevicePtr_t robot)
......@@ -151,8 +151,8 @@ void test_difference_and_distance(DevicePtr_t robot)
q0 = se3::randomConfiguration (robot->model());
q1 = se3::randomConfiguration (robot->model());
BOOST_CHECK(isValidConfiguration(robot, q0, eps));
BOOST_CHECK(isValidConfiguration(robot, q1, eps));
BOOST_CHECK(isNormalized(robot, q0, eps));
BOOST_CHECK(isNormalized(robot, q1, eps));
difference<se3::LieGroupTpl> (robot, q1, q0, q1_minus_q0);
......@@ -190,13 +190,13 @@ void test_difference_and_integrate(DevicePtr_t robot)
q0 = se3::randomConfiguration (robot->model());
q1 = se3::randomConfiguration (robot->model());
BOOST_CHECK(isValidConfiguration(robot, q0, eps));
BOOST_CHECK(isValidConfiguration(robot, q1, eps));
BOOST_CHECK(isNormalized(robot, q0, eps));
BOOST_CHECK(isNormalized(robot, q1, eps));
difference<LieGroup> (robot, q1, q0, q1_minus_q0);
integrate<true, LieGroup> (robot, q0, q1_minus_q0, q2);
BOOST_CHECK(isValidConfiguration(robot, q2, eps));
BOOST_CHECK(isNormalized(robot, q2, eps));
// Check that distance (q0 + (q1 - q0), q1) is zero
BOOST_CHECK_MESSAGE (isApprox(robot, q1, q2, eps),
......@@ -230,8 +230,8 @@ void test_interpolate_and_integrate (DevicePtr_t robot)
q0 = se3::randomConfiguration (robot->model());
q1 = se3::randomConfiguration (robot->model());
BOOST_CHECK(isValidConfiguration(robot, q0, eps));
BOOST_CHECK(isValidConfiguration(robot, q1, eps));
BOOST_CHECK(isNormalized(robot, q0, eps));
BOOST_CHECK(isNormalized(robot, q1, eps));
difference<LieGroup> (robot, q1, q0, q1_minus_q0);
......@@ -264,8 +264,8 @@ void test_interpolate_and_integrate (DevicePtr_t robot)
interpolate<LieGroup> (robot, q0, q1, 0.5, q2);
integrate<true, LieGroup> (robot, q0, 0.5 * q1_minus_q0, q3);
BOOST_CHECK(isValidConfiguration(robot, q2, eps));
BOOST_CHECK(isValidConfiguration(robot, q3, eps));
BOOST_CHECK(isNormalized(robot, q2, eps));
BOOST_CHECK(isNormalized(robot, q3, eps));
BOOST_CHECK(isApprox(robot, q2, q3, eps));
......@@ -303,11 +303,11 @@ void test_successive_interpolation (DevicePtr_t robot)
for (size_type i=0; i<NB_SUCCESSIVE_INTERPOLATION; ++i) {
q1 = se3::randomConfiguration (robot->model());
BOOST_CHECK(isValidConfiguration(robot, q1, eps));
BOOST_CHECK(isNormalized(robot, q1, eps));
difference<LieGroup> (robot, q1, q0, q1_minus_q0);
integrate<true, LieGroup> (robot, q0, 0.5 * q1_minus_q0, q0);
BOOST_CHECK(isValidConfiguration(robot, q0, eps));
BOOST_CHECK(isNormalized(robot, q0, eps));
}
}
}
......
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