Commit 495132df authored by Justin Carpentier's avatar Justin Carpentier Committed by GitHub
Browse files

Merge pull request #410 from jcarpent/topic/devel

Fix bug in distance computation
parents d03e6adc b9a80b6c
...@@ -58,12 +58,19 @@ namespace se3 ...@@ -58,12 +58,19 @@ namespace se3
"Difference between two configurations, ie. the tangent vector that must be integrated during one unit time" "Difference between two configurations, ie. the tangent vector that must be integrated during one unit time"
"to go from q1 to q2"); "to go from q1 to q2");
bp::def("squaredDistance",
(VectorXd (*)(const Model &, const VectorXd &, const VectorXd &))&squaredDistance,
bp::args("Model",
"Configuration q1 (size Model::nq)",
"Configuration q2 (size Model::nq)"),
"Squared distance vector between two configurations.");
bp::def("distance", bp::def("distance",
(VectorXd (*)(const Model &, const VectorXd &, const VectorXd &))&distance, (double (*)(const Model &, const VectorXd &, const VectorXd &))&distance,
bp::args("Model", bp::args("Model",
"Configuration q1 (size Model::nq)", "Configuration q1 (size Model::nq)",
"Configuration q2 (size Model::nq)"), "Configuration q2 (size Model::nq)"),
"Distance between two configurations "); "Distance between two configurations.");
bp::def("randomConfiguration", bp::def("randomConfiguration",
(VectorXd (*)(const Model &, const VectorXd &, const VectorXd &))&randomConfiguration, (VectorXd (*)(const Model &, const VectorXd &, const VectorXd &))&randomConfiguration,
......
...@@ -26,22 +26,23 @@ ...@@ -26,22 +26,23 @@
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(se3::GeometryData) EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(se3::GeometryData)
namespace fcl //namespace fcl
{ //{
#ifdef WITH_HPP_FCL //#ifdef WITH_HPP_FCL
// This operator is defined here temporary, as it is needed by vector_indexing_suite // // This operator is defined here temporary, as it is needed by vector_indexing_suite
// It has also been defined in hpp-fcl in a pending pull request. // // It has also been defined in hpp-fcl in a pending pull request.
// Once it has been integrated in releases of hpp-fcl, please remove this operator // // Once it has been integrated in releases of hpp-fcl, please remove this operator
inline bool operator ==(const DistanceResult & dr1, const DistanceResult& dr2) // inline bool operator ==(const DistanceResult & dr1, const DistanceResult& dr2)
{ // {
return dr1.min_distance == dr2.min_distance // return dr1.min_distance == dr2.min_distance
&& dr1.o1 == dr2.o1 // && dr1.o1 == dr2.o1
&& dr1.o2 == dr2.o2 // && dr1.o2 == dr2.o2
&& dr1.nearest_points[0] == dr2.nearest_points[0] // && dr1.nearest_points[0] == dr2.nearest_points[0]
&& dr1.nearest_points[1] == dr2.nearest_points[1]; // && dr1.nearest_points[1] == dr2.nearest_points[1];
} // }
#endif //#endif
} //}
namespace se3 namespace se3
{ {
namespace python namespace python
......
...@@ -23,6 +23,8 @@ ...@@ -23,6 +23,8 @@
#include "pinocchio/multibody/liegroup/liegroup.hpp" #include "pinocchio/multibody/liegroup/liegroup.hpp"
#include <cmath>
namespace se3 namespace se3
{ {
...@@ -410,6 +412,23 @@ namespace se3 ...@@ -410,6 +412,23 @@ namespace se3
{ {
return squaredDistance<LieGroupTpl>(model, q0, q1); return squaredDistance<LieGroupTpl>(model, q0, q1);
} }
template<typename LieGroup_t>
inline double
distance(const Model & model,
const Eigen::VectorXd & q0,
const Eigen::VectorXd & q1)
{
return std::sqrt(squaredDistance<LieGroup_t>(model, q0, q1).sum());
}
inline double
distance(const Model & model,
const Eigen::VectorXd & q0,
const Eigen::VectorXd & q1)
{
return std::sqrt(squaredDistance<LieGroupTpl>(model, q0, q1).sum());
}
template<typename LieGroup_t, typename JointModel> struct RandomConfigurationStepAlgo; template<typename LieGroup_t, typename JointModel> struct RandomConfigurationStepAlgo;
......
...@@ -329,6 +329,19 @@ BOOST_AUTO_TEST_CASE ( neutral_configuration_test ) ...@@ -329,6 +329,19 @@ BOOST_AUTO_TEST_CASE ( neutral_configuration_test )
BOOST_CHECK_MESSAGE(model.neutralConfiguration.isApprox(expected, 1e-12), "neutral configuration - wrong results"); BOOST_CHECK_MESSAGE(model.neutralConfiguration.isApprox(expected, 1e-12), "neutral configuration - wrong results");
} }
BOOST_AUTO_TEST_CASE ( distance_configuration_test )
{
Model model; buildModel(model);
Eigen::VectorXd q0(model.neutralConfiguration);
Eigen::VectorXd q1(q0 + Eigen::VectorXd::Ones(model.nq));
double dist = distance(model,q0,q1);
BOOST_CHECK_MESSAGE(dist > 0., "distance - wrong results");
BOOST_CHECK_SMALL(dist-differentiate(model,q0,q1).norm(), 1e-12);
}
BOOST_AUTO_TEST_CASE ( uniform_sampling_test ) BOOST_AUTO_TEST_CASE ( uniform_sampling_test )
{ {
Model model; buildModel(model); Model model; buildModel(model);
......
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