[Python] Start to expose FCL objects

bindings/python/CMakeLists.txt

 # 
-# Copyright (c) 2015-2016 CNRS
+# Copyright (c) 2015-2017 CNRS
 # Copyright (c) 2015 Wandercraft, 86 rue de Paris 91400 Orsay, France.
 # 
 # This file is part of Pinocchio
@@ -83,6 +83,10 @@ SET(${PROJECT_NAME}_PYTHON_HEADERS
   multibody/joint/joints-variant.hpp
   multibody/joint/joint.hpp
   multibody/joint/joint-derived.hpp
+  multibody/fcl/contact.hpp
+  multibody/fcl/collision-result.hpp
+  multibody/fcl/distance-result.hpp
+  multibody/fcl/collision-geometry.hpp
   algorithm/algorithms.hpp 
   parsers/parsers.hpp
   )
@@ -99,6 +103,7 @@ SET(${PROJECT_NAME}_PYTHON_SOURCES
   multibody/expose-data.cpp
   multibody/expose-geometry.cpp
   multibody/joint/expose-joints.cpp
+  multibody/fcl/expose-fcl.cpp
   algorithm/expose-algorithms.cpp
   algorithm/expose-com.cpp
   algorithm/expose-kinematics.cpp
@@ -131,6 +136,7 @@ MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/bindings/python"
 MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/bindings/python/spatial")
 MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/bindings/python/multibody")
 MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/bindings/python/multibody/joint")
+MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/bindings/python/multibody/fcl")
 MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/bindings/python/parsers")
 MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/bindings/python/algorithm")
 MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/bindings/python/utils")

bindings/python/fwd.hpp

 //
-// Copyright (c) 2015-2016 CNRS
+// Copyright (c) 2015-2017 CNRS
 // Copyright (c) 2015 Wandercraft, 86 rue de Paris 91400 Orsay, France.
 //
 // This file is part of Pinocchio
@@ -44,6 +44,10 @@ namespace se3
     
     // Expose algorithms
     void exposeAlgorithms();
+    
+#ifdef WITH_HPP_FCL
+    void exposeFCL();
+#endif // WITH_HPP_FCL
 
   } // namespace python
 } // namespace se3

bindings/python/module.cpp

@@ -56,5 +56,9 @@ BOOST_PYTHON_MODULE(libpinocchio_pywrap)
   exposeAlgorithms();
   exposeParsers();
   
+#ifdef WITH_HPP_FCL
+  exposeFCL();
+#endif // WITH_HPP_FCL
+  
 }
  

bindings/python/multibody/fcl/collision-geometry.hpp

+//
+// Copyright (c) 2017 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_collision_geometry_hpp__
+#define __se3_python_fcl_collision_geometry_hpp__
+
+#include "pinocchio/spatial/fcl-pinocchio-conversions.hpp"
+#include <hpp/fcl/collision_object.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 CollisionGeometryPythonVisitor : public bp::def_visitor<CollisionGeometryPythonVisitor>
+      {
+        typedef ::fcl::CollisionGeometry CollisionGeometry;
+        
+        template<class PyClass>
+        void visit(PyClass& cl) const
+        {
+          cl
+          
+          .def("getObjectType",&CollisionGeometry::getObjectType,"Get the type of the object.")
+          .def("getNodeType",&CollisionGeometry::getNodeType,"Get the node type.")
+          
+          .def("computeLocalAABB",&CollisionGeometry::computeLocalAABB)
+          .def("isOccupied",&CollisionGeometry::isOccupied)
+          .def("isFree",&CollisionGeometry::isFree)
+          .def("isUncertain",&CollisionGeometry::isUncertain)
+          
+          .def_readwrite("aabb_radius",&CollisionGeometry::aabb_radius,"AABB radius")
+          ;
+        }
+        
+        static void expose()
+        {
+          bp::class_<CollisionGeometry,boost::noncopyable>("CollisionGeometry",
+                                                           "The geometry for the object for collision or distance computation.",
+                                                           bp::no_init)
+          .def(CollisionGeometryPythonVisitor())
+          ;
+          
+          bp::enum_< ::fcl::OBJECT_TYPE >("OBJECT_TYPE")
+          .value("OT_UNKNOWN",::fcl::OT_UNKNOWN)
+          .value("OT_BVH",::fcl::OT_BVH)
+          .value("OT_GEOM",::fcl::OT_GEOM)
+          .value("OT_OCTREE",::fcl::OT_OCTREE)
+          .value("OT_COUNT",::fcl::OT_COUNT)
+          ;
+          
+          bp::enum_< ::fcl::NODE_TYPE >("NODE_TYPE")
+          .value("BV_UNKNOWN",::fcl::BV_UNKNOWN)
+          .value("BV_AABB",::fcl::BV_AABB)
+          .value("BV_OBB",::fcl::BV_OBB)
+          .value("BV_RSS",::fcl::BV_RSS)
+          .value("BV_kIOS",::fcl::BV_kIOS)
+          .value("BV_OBBRSS",::fcl::BV_OBBRSS)
+          .value("BV_KDOP16",::fcl::BV_KDOP16)
+          .value("BV_KDOP18",::fcl::BV_KDOP18)
+          .value("BV_KDOP24",::fcl::BV_KDOP24)
+          .value("GEOM_BOX",::fcl::GEOM_BOX)
+          .value("GEOM_SPHERE",::fcl::GEOM_SPHERE)
+          .value("GEOM_CAPSULE",::fcl::GEOM_CAPSULE)
+          .value("GEOM_CONE",::fcl::GEOM_CONE)
+          .value("GEOM_CYLINDER",::fcl::GEOM_CYLINDER)
+          .value("GEOM_CONVEX",::fcl::GEOM_CONVEX)
+          .value("GEOM_PLANE",::fcl::GEOM_PLANE)
+          .value("GEOM_HALFSPACE",::fcl::GEOM_HALFSPACE)
+          .value("GEOM_TRIANGLE",::fcl::GEOM_TRIANGLE)
+          .value("GEOM_OCTREE",::fcl::GEOM_OCTREE)
+          .value("NODE_COUNT",::fcl::NODE_COUNT)
+          ;
+        }
+        
+      private:
+        
+        
+      };
+      
+    } // namespace fcl
+    
+  } // namespace python
+} // namespace se3
+
+#endif // namespace __se3_python_fcl_collision_geometry_hpp__

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

+//
+// Copyright (c) 2017 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_collision_result_hpp__
+#define __se3_python_fcl_collision_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 CollisionResultPythonVisitor : public bp::def_visitor<CollisionResultPythonVisitor>
+      {
+        typedef ::fcl::CollisionResult CollisionResult;
+        
+        template<class PyClass>
+        void visit(PyClass& cl) const
+        {
+          cl
+          .def(bp::init<>("Default constructor"))
+          
+          .def("addContact",&CollisionResult::addContact,bp::arg("contact"),"Adds one contact into result structure")
+          .def("isCollision",&CollisionResult::isCollision,"Returns binary collision result")
+          .def("numContacts",&CollisionResult::numContacts,"Returns the number of contacts found")
+          .def("numCostSources",&CollisionResult::numCostSources,"Returns the number of cost sources found")
+          
+          .def("clear",&CollisionResult::clear,"Clears the results obtained")
+          
+          .def("getContacts",&CollisionResult::getContacts,"Get all the contacts",bp::return_internal_reference<>())
+          .def("getContact",&CollisionResult::getContact,bp::arg("index"),"Get the i-th contact calculated",bp::return_value_policy<bp::copy_const_reference>())
+          
+          .def_readwrite("distance_lower_bound",&CollisionResult::distance_lower_bound,"Lower bound on distance between objects if they are disjoint (computed only on request).")
+          
+          ;
+        }
+        
+        static void expose()
+        {
+          bp::class_<CollisionResult>("CollisionResult",
+                              "Contact information returned by collision.",
+                              bp::no_init)
+          .def(CollisionResultPythonVisitor())
+          ;
+        }
+        
+      private:
+        
+        
+      };
+      
+    } // namespace fcl
+    
+  } // namespace python
+} // namespace se3
+
+#endif // namespace __se3_python_fcl_collision_result_hpp__

bindings/python/multibody/fcl/contact.hpp

+//
+// Copyright (c) 2017 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_contact_hpp__
+#define __se3_python_fcl_contact_hpp__
+
+#include "pinocchio/spatial/fcl-pinocchio-conversions.hpp"
+#include <hpp/fcl/collision_data.h>
+
+#include <boost/python.hpp>
+#include <boost/python/copy_const_reference.hpp>
+#include <boost/python/suite/indexing/vector_indexing_suite.hpp>
+
+namespace se3
+{
+  namespace python
+  {
+    namespace fcl
+    {
+    
+      namespace bp = boost::python;
+      
+      struct ContactPythonVisitor : public bp::def_visitor<ContactPythonVisitor>
+      {
+        typedef ::fcl::Contact Contact;
+        typedef ::fcl::CollisionGeometry CollisionGeometry;
+        
+        template<class PyClass>
+        void visit(PyClass& cl) const
+        {
+          cl
+          .def(bp::init<>("Default constructor"))
+          // TODO: add a kind of bp::with_custodian_and_ward_postcall<0,1>() for managing arguments lifetime
+          .def("__init__",bp::make_constructor(&make_contact,
+                                               bp::default_call_policies(),
+                                               bp::args("o1","o2","b1","b2")),
+               "Constructor from two Collision Geometry objects")
+          
+          .def_readwrite("b1", &Contact::b1, "Contact primitive in object 1.\n"
+                         "if object 1 is mesh or point cloud, it is the triangle or point id\n"
+                         "if object 1 is geometry shape, it is NONE (-1),\n"
+                         "if object 1 is octree, it is the id of the cell.")
+          
+          .def_readwrite("b2", &Contact::b2, "Contact primitive in object 2.\n"
+                         "if object 2 is mesh or point cloud, it is the triangle or point id\n"
+                         "if object 2 is geometry shape, it is NONE (-1),\n"
+                         "if object 2 is octree, it is the id of the cell.")
+          .add_property("normal", &getNormal, &setNormal,
+                        "Contact normal, pointing from o1 to o2.")
+          .add_property("pos", &getPos, &setPos,
+                        "Contact position, in world space.")
+          
+          .def_readwrite("penetration_depth",&Contact::penetration_depth,"Penetration depth.")
+          
+          .add_property("o1",bp::make_function(&getObject1,bp::return_value_policy<bp::copy_const_reference>()),"Returns a copy of collision object 1")
+          .add_property("o2",bp::make_function(&getObject2,bp::return_value_policy<bp::copy_const_reference>()),"Returns a copy of collision object 2.")
+          ;
+        }
+        
+        static void expose()
+        {
+          bp::class_<Contact>("Contact",
+                              "Contact information returned by collision.",
+                              bp::no_init)
+          .def(ContactPythonVisitor())
+          ;
+        }
+        
+      private:
+        
+        static Contact * make_contact(const CollisionGeometry & o1, const CollisionGeometry & o2, int b1, int b2)
+        { return new Contact(&o1,&o2,b1,b2); }
+        
+        static Eigen::Vector3d getNormal(const Contact & self)
+        { return toVector3d(self.normal); }
+        static void setNormal(Contact & self, const Eigen::Vector3d & normal)
+        { self.normal = toFclVec3f(normal); }
+        
+        static Eigen::Vector3d getPos(const Contact & self)
+        { return toVector3d(self.pos); }
+        static void setPos(Contact & self, const Eigen::Vector3d & pos)
+        { self.pos = toFclVec3f(pos); }
+        
+        static const CollisionGeometry & getObject1(const Contact & self)
+        { return *self.o1; }
+        static const CollisionGeometry & getObject2(const Contact & self)
+        { return *self.o2; }
+      };
+      
+    } // namespace fcl
+    
+  } // namespace python
+} // namespace se3
+
+#endif // namespace __se3_python_fcl_contact_hpp__

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

+//
+// Copyright (c) 2017 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/>.
+
+#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/collision-geometry.hpp"
+
+#include "pinocchio/bindings/python/utils/std-vector.hpp"
+
+namespace se3
+{
+  namespace python
+  {
+    void exposeFCL()
+    {
+      using namespace se3::python::fcl;
+      ContactPythonVisitor::expose();
+      StdVectorPythonVisitor<ContactPythonVisitor::Contact>::expose("StdVect_Contact");
+      
+      CollisionResultPythonVisitor::expose();
+      
+      CollisionGeometryPythonVisitor::expose();
+    }
+    
+  } // namespace python
+} // namespace se3