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
"Difference between two configurations, ie. the tangent vector that must be integrated during one unit time"
"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",
(VectorXd (*)(const Model &, const VectorXd &, const VectorXd &))&distance,
(double (*)(const Model &, const VectorXd &, const VectorXd &))&distance,
bp::args("Model",
"Configuration q1 (size Model::nq)",
"Configuration q2 (size Model::nq)"),
"Distance between two configurations ");
"Distance between two configurations.");
bp::def("randomConfiguration",
(VectorXd (*)(const Model &, const VectorXd &, const VectorXd &))&randomConfiguration,
......
......@@ -26,22 +26,23 @@
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(se3::GeometryData)
namespace fcl
{
#ifdef WITH_HPP_FCL
// 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.
// Once it has been integrated in releases of hpp-fcl, please remove this operator
inline bool operator ==(const DistanceResult & dr1, const DistanceResult& dr2)
{
return dr1.min_distance == dr2.min_distance
&& dr1.o1 == dr2.o1
&& dr1.o2 == dr2.o2
&& dr1.nearest_points[0] == dr2.nearest_points[0]
&& dr1.nearest_points[1] == dr2.nearest_points[1];
}
#endif
}
//namespace fcl
//{
//#ifdef WITH_HPP_FCL
// // 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.
// // Once it has been integrated in releases of hpp-fcl, please remove this operator
// inline bool operator ==(const DistanceResult & dr1, const DistanceResult& dr2)
// {
// return dr1.min_distance == dr2.min_distance
// && dr1.o1 == dr2.o1
// && dr1.o2 == dr2.o2
// && dr1.nearest_points[0] == dr2.nearest_points[0]
// && dr1.nearest_points[1] == dr2.nearest_points[1];
// }
//#endif
//}
namespace se3
{
namespace python
......
......@@ -23,6 +23,8 @@
#include "pinocchio/multibody/liegroup/liegroup.hpp"
#include <cmath>
namespace se3
{
......@@ -410,6 +412,23 @@ namespace se3
{
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;
......
......@@ -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_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 )
{
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