Merge pull request #410 from jcarpent/topic/devel

Fix bug in distance computation

bindings/python/algorithm/expose-joints.cpp

@@ -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,

bindings/python/multibody/geometry-data.hpp

@@ -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

src/algorithm/joint-configuration.hpp

@@ -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;
 

unittest/joint-configurations.cpp

@@ -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);