[Python] Expose DistanceResult from FCL

bindings/python/CMakeLists.txt

@@ -116,7 +116,7 @@ IF(HPP_FCL_FOUND)
   SET(${PROJECT_NAME}_PYTHON_FCL_HEADERS
     multibody/fcl/contact.hpp
     multibody/fcl/collision-result.hpp
-  #  multibody/fcl/distance-result.hpp
+    multibody/fcl/distance-result.hpp
     multibody/fcl/collision-geometry.hpp
     )
 

bindings/python/multibody/fcl/distance-result.hpp

+//
+// Copyright (c) 2018 CNRS
+//
+// This file is part of Pinocchio
+// Pinocchio is free software: you can redistribute it
+// and/or modify it under the terms of the GNU Lesser General Public
+// License as published by the Free Software Foundation, either version
+// 3 of the License, or (at your option) any later version.
+//
+// Pinocchio is distributed in the hope that it will be
+// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
+// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+// General Lesser Public License for more details. You should have
+// received a copy of the GNU Lesser General Public License along with
+// Pinocchio If not, see
+// <http://www.gnu.org/licenses/>.
+
+#ifndef __se3_python_fcl_distance_result_hpp__
+#define __se3_python_fcl_distance_result_hpp__
+
+#include "pinocchio/spatial/fcl-pinocchio-conversions.hpp"
+#include <hpp/fcl/collision_data.h>
+
+#include <boost/python/copy_const_reference.hpp>
+#include <boost/python/return_internal_reference.hpp>
+#include <boost/python/suite/indexing/vector_indexing_suite.hpp>
+
+namespace se3
+{
+  namespace python
+  {
+    namespace fcl
+    {
+      
+      namespace bp = boost::python;
+      
+      struct DistanceResultPythonVisitor : public bp::def_visitor<DistanceResultPythonVisitor>
+      {
+        typedef ::fcl::DistanceResult DistanceResult;
+        
+        template<class PyClass>
+        void visit(PyClass& cl) const
+        {
+          cl
+          .def(bp::init<>("Default constructor"))
+          
+          .def_readonly("min_distance",&DistanceResult::min_distance,
+                        "minimum distance between two objects.\n"
+                        "If two objects are in collision, min_distance <= 0.")
+          
+          .def("getNearestPoint1",getNearestPoint1,
+               "Returns the nearest point on collision object 1.")
+          .def("getNearestPoint2",getNearestPoint2,
+               "Returns the nearest point on collision object 2.")
+          
+          ;
+        }
+        
+        static void expose()
+        {
+          bp::class_<DistanceResult>("DistanceResult",
+                                      "Contact information returned by collision.",
+                                      bp::no_init)
+          .def(DistanceResultPythonVisitor())
+          ;
+        }
+        
+      private:
+        
+        static ::fcl::Vec3f getNearestPoint1(const DistanceResult & res) { return res.nearest_points[0]; }
+        static ::fcl::Vec3f getNearestPoint2(const DistanceResult & res) { return res.nearest_points[1]; }
+        
+      };
+      
+    } // namespace fcl
+    
+  } // namespace python
+} // namespace se3
+
+#endif // namespace __se3_python_fcl_distance_result_hpp__

bindings/python/multibody/fcl/expose-fcl.cpp

 //
-// Copyright (c) 2017 CNRS
+// Copyright (c) 2017-2018 CNRS
 //
 // This file is part of Pinocchio
 // Pinocchio is free software: you can redistribute it
@@ -18,6 +18,7 @@
 #include "pinocchio/bindings/python/fwd.hpp"
 #include "pinocchio/bindings/python/multibody/fcl/contact.hpp"
 #include "pinocchio/bindings/python/multibody/fcl/collision-result.hpp"
+#include "pinocchio/bindings/python/multibody/fcl/distance-result.hpp"
 #include "pinocchio/bindings/python/multibody/fcl/collision-geometry.hpp"
 
 #include "pinocchio/bindings/python/utils/std-vector.hpp"
@@ -35,6 +36,9 @@ namespace se3
       CollisionResultPythonVisitor::expose();
       StdVectorPythonVisitor<CollisionResultPythonVisitor::CollisionResult>::expose("StdVect_CollisionResult");
       
+      DistanceResultPythonVisitor::expose();
+      StdVectorPythonVisitor<DistanceResultPythonVisitor::DistanceResult>::expose("StdVect_DistanceResult");
+      
       CollisionGeometryPythonVisitor::expose();
     }
     

bindings/python/multibody/geometry-data.hpp

 //
-// Copyright (c) 2015-2017 CNRS
+// Copyright (c) 2015-2018 CNRS
 //
 // This file is part of Pinocchio
 // Pinocchio is free software: you can redistribute it
@@ -112,10 +112,6 @@ namespace se3
       /* --- Expose --------------------------------------------------------- */
       static void expose()
       {
-#ifdef WITH_HPP_FCL
-        bp::class_< std::vector<fcl::DistanceResult> >("StdVec_DistanceResult")
-        .def(bp::vector_indexing_suite< std::vector<fcl::DistanceResult> >());
-#endif // WITH_HPP_FCL
         bp::class_<GeometryData>("GeometryData",
                                  "Geometry data linked to a geometry model and data struct.",
                                  bp::no_init)