[CMake][Python] Exposed GeometryModel and GeometryData in python. Little...

[CMake][Python] Exposed GeometryModel and GeometryData in python. Little reworked of the headers_variables in the cmakelists

CMakeLists.txt

@@ -161,67 +161,63 @@ SET(${PROJECT_NAME}_PYTHON_HEADERS
   python/explog.hpp
   )
 
-SET(HEADERS
-  ${${PROJECT_NAME}_MATH_HEADERS}
-  ${${PROJECT_NAME}_TOOLS_HEADERS}
-  ${${PROJECT_NAME}_SPATIAL_HEADERS}
-  ${${PROJECT_NAME}_MULTIBODY_JOINT_HEADERS}
-  ${${PROJECT_NAME}_MULTIBODY_PARSER_HEADERS}
-  ${${PROJECT_NAME}_MULTIBODY_HEADERS}
-  ${${PROJECT_NAME}_ALGORITHM_HEADERS}
-  ${${PROJECT_NAME}_SIMULATION_HEADERS}
-  ${${PROJECT_NAME}_PYTHON_HEADERS}
-  exception.hpp
-  assert.hpp
- )
+IF(HPP_FCL_FOUND)
+  LIST(APPEND ${PROJECT_NAME}_PYTHON_HEADERS
+      python/geometry-model.hpp
+      python/geometry-data.hpp
+    )
+ENDIF(HPP_FCL_FOUND)
 
 IF(URDFDOM_FOUND)
-  SET(${PROJECT_NAME}_MULTIBODY_PARSER_HEADERS
-    ${${PROJECT_NAME}_MULTIBODY_PARSER_HEADERS}
+  LIST(APPEND ${PROJECT_NAME}_MULTIBODY_PARSER_HEADERS
     multibody/parser/urdf.hpp
     multibody/parser/urdf.hxx
     )
   
   IF(HPP_FCL_FOUND )
-    SET(${PROJECT_NAME}_MULTIBODY_PARSER_HEADERS
-    ${${PROJECT_NAME}_MULTIBODY_PARSER_HEADERS}
-    multibody/parser/from-collada-to-fcl.hpp
-    multibody/parser/urdf-with-geometry.hpp
-    )
+    LIST(APPEND ${PROJECT_NAME}_MULTIBODY_PARSER_HEADERS
+      multibody/parser/from-collada-to-fcl.hpp
+      multibody/parser/urdf-with-geometry.hpp
+      )
   ENDIF(HPP_FCL_FOUND)
 
-  LIST(APPEND HEADERS
-    ${${PROJECT_NAME}_MULTIBODY_PARSER_HEADERS}
-    )
   ADD_DEFINITIONS(-DWITH_URDFDOM)
 ENDIF(URDFDOM_FOUND)
 
 IF(HPP_FCL_FOUND)
-  SET(${PROJECT_NAME}_SPATIAL_HEADERS
-    ${${PROJECT_NAME}_SPATIAL_HEADERS}
+  LIST(APPEND ${PROJECT_NAME}_SPATIAL_HEADERS
     spatial/fcl-pinocchio-conversions.hpp
     )
-  SET(${PROJECT_NAME}_MULTIBODY_HEADERS
-    ${${PROJECT_NAME}_MULTIBODY_HEADERS}
+  LIST(APPEND ${PROJECT_NAME}_MULTIBODY_HEADERS
     multibody/geometry.hpp
     )
-  LIST(APPEND HEADERS
-    ${${PROJECT_NAME}_MULTIBODY_HEADERS}
-    )
 ENDIF(HPP_FCL_FOUND)
 
 IF(LUA5_1_FOUND)
-  SET(${PROJECT_NAME}_MULTIBODY_PARSER_LUA_HEADERS
+  LIST(APPEND ${PROJECT_NAME}_MULTIBODY_PARSER_HEADERS
     multibody/parser/lua.hpp
     multibody/parser/lua/lua_tables.hpp
     )
 
-  LIST(APPEND HEADERS 
-    ${${PROJECT_NAME}_MULTIBODY_PARSER_LUA_HEADERS}
-    )
   ADD_DEFINITIONS(-DWITH_LUA)
 ENDIF(LUA5_1_FOUND)
 
+
+SET(HEADERS
+  ${${PROJECT_NAME}_MATH_HEADERS}
+  ${${PROJECT_NAME}_TOOLS_HEADERS}
+  ${${PROJECT_NAME}_SPATIAL_HEADERS}
+  ${${PROJECT_NAME}_MULTIBODY_JOINT_HEADERS}
+  ${${PROJECT_NAME}_MULTIBODY_PARSER_HEADERS}
+  ${${PROJECT_NAME}_MULTIBODY_HEADERS}
+  ${${PROJECT_NAME}_ALGORITHM_HEADERS}
+  ${${PROJECT_NAME}_SIMULATION_HEADERS}
+  ${${PROJECT_NAME}_PYTHON_HEADERS}
+  exception.hpp
+  assert.hpp
+ )
+LIST(REMOVE_DUPLICATES HEADERS)
+
 PKG_CONFIG_APPEND_LIBS (${PROJECT_NAME})
 
 MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio")
@@ -236,6 +232,7 @@ MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/algorithm")
 MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/simulation")
 MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/python")
 
+
 FOREACH(header ${HEADERS})
   GET_FILENAME_COMPONENT(headerName ${header} NAME)
   GET_FILENAME_COMPONENT(headerPath ${header} PATH)

src/multibody/geometry.hpp

@@ -72,6 +72,14 @@ namespace se3
     // Get closest point on outer object in global frame,
     Eigen::Vector3d closestPointOuter () const { return toVector3d(fcl_distance_result.nearest_points [1]); }
     
+    bool operator == (const DistanceResult & other) const
+    {
+      return (distance() == other.distance()
+        && closestPointInner() == other.closestPointInner()
+        && closestPointOuter() == other.closestPointOuter()
+        && object1 == other.object1
+        && object2 == other.object2);
+    }
     fcl::DistanceResult fcl_distance_result;
     std::size_t object1;
     std::size_t object2;

src/python/geometry-data.hpp

+//
+// Copyright (c) 2015 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_geometry_data_hpp__
+#define __se3_python_geometry_data_hpp__
+
+#include <boost/python/suite/indexing/vector_indexing_suite.hpp>
+#include <eigenpy/exception.hpp>
+#include <eigenpy/eigenpy.hpp>
+
+// #include "pinocchio/multibody/parser/sample-models.hpp"
+#include "pinocchio/python/se3.hpp"
+#include "pinocchio/python/eigen_container.hpp"
+#include "pinocchio/python/handler.hpp"
+
+#include "pinocchio/multibody/geometry.hpp"
+
+namespace se3
+{
+  namespace python
+  {
+    namespace bp = boost::python;
+
+    typedef Handler<GeometryData> GeometryDataHandler;
+
+    struct GeometryDataPythonVisitor
+      : public boost::python::def_visitor< GeometryDataPythonVisitor >
+    {
+    public:
+      typedef GeometryData::Index Index;
+      typedef GeometryData::CollisionPair_t CollisionPair_t;
+      typedef se3::DistanceResult DistanceResult;
+      typedef eigenpy::UnalignedEquivalent<SE3>::type SE3_fx;
+      
+    public:
+
+      /* --- Convert From C++ to Python ------------------------------------- */
+      // static PyObject* convert(Model const& modelConstRef)
+      // {
+      // 	Model * ptr = const_cast<Model*>(&modelConstRef);
+      // 	return boost::python::incref(boost::python::object(ModelHandler(ptr)).ptr());
+      // }
+      static PyObject* convert(GeometryDataHandler::SmartPtr_t const& ptr)
+      {
+        return boost::python::incref(boost::python::object(GeometryDataHandler(ptr)).ptr());
+      }
+
+      /* --- Exposing C++ API to python through the handler ----------------- */
+    template<class PyClass>
+      void visit(PyClass& cl) const 
+      {
+	cl
+    .add_property("nCollisionPairs", &GeometryDataPythonVisitor::nCollisionPairs)
+
+    .add_property("oMg",
+      bp::make_function(&GeometryDataPythonVisitor::oMg,
+            bp::return_internal_reference<>())  )
+    .add_property("collision_pairs",
+      bp::make_function(&GeometryDataPythonVisitor::collision_pairs,
+            bp::return_internal_reference<>())  )
+    .add_property("distances",
+      bp::make_function(&GeometryDataPythonVisitor::distances,
+            bp::return_internal_reference<>())  )
+
+    .def("addCollisionPair",&GeometryDataPythonVisitor::isCollisionPair)
+    .def("removeCollisionPair",&GeometryDataPythonVisitor::isCollisionPair)
+    .def("isCollisionPair",&GeometryDataPythonVisitor::isCollisionPair)
+    .def("collide",&GeometryDataPythonVisitor::collide)
+    .def("isColliding",&GeometryDataPythonVisitor::isColliding)
+    .def("computeDistances",&GeometryDataPythonVisitor::computeDistances)
+ 
+
+    .def("__str__",&GeometryDataPythonVisitor::toString)
+
+
+	  ;
+      }
+
+      static GeometryModel::Index nCollisionPairs( GeometryDataHandler & m ) { return m->nCollisionPairs; }
+      
+      static std::vector<SE3> & oMg( GeometryDataHandler & m ) { return m->oMg; }
+      static std::vector<CollisionPair_t> & collision_pairs( GeometryDataHandler & m ) { return m->collision_pairs; }
+      static std::vector<DistanceResult> & distances( GeometryDataHandler & m ) { return m->distances; }
+
+      static void addCollisionPair (GeometryDataHandler & m, Index co1, Index co2) { m -> addCollisionPair(co1, co2);}
+      static void removeCollisionPair (GeometryDataHandler & m, Index co1, Index co2) { m -> removeCollisionPair(co1, co2);}
+      static bool isCollisionPair (const GeometryDataHandler & m, Index co1, Index co2) { return m -> isCollisionPair(co1, co2);}
+
+      static bool collide(const GeometryDataHandler & m, Index co1, Index co2) { return m -> collide(co1, co2); };
+      static bool isColliding(const GeometryDataHandler & m ) { return m -> isColliding(); };
+
+      static void computeDistances (GeometryDataHandler & m) { m -> computeDistances(); }
+
+      
+ 
+
+      static std::string toString(const GeometryDataHandler& m) 
+      {	  std::ostringstream s; s << *m; return s.str();       }
+
+      /* --- Expose --------------------------------------------------------- */
+      static void expose()
+      {
+  
+  bp::class_<GeometryDataHandler>("GeometryData",
+         "Geometry data ",
+         bp::no_init)
+    .def(GeometryDataPythonVisitor());
+
+    bp::class_< std::vector<CollisionPair_t> >("StdVec_CollisionPair_t")
+          .def(bp::vector_indexing_suite< std::vector<CollisionPair_t> >());
+    bp::class_< std::vector<DistanceResult> >("StdVec_DistanceResult")
+          .def(bp::vector_indexing_suite< std::vector<DistanceResult> >());
+      
+	/* Not sure if it is a good idea to enable automatic
+	 * conversion. Prevent it for now */
+	//bp::to_python_converter< Model,GeometryDataPythonVisitor >();
+	bp::to_python_converter< GeometryDataHandler::SmartPtr_t,GeometryDataPythonVisitor >();
+      }
+
+
+    };
+    
+
+
+  }} // namespace se3::python
+
+#endif // ifndef __se3_python_geometry_data_hpp__
+

src/python/geometry-model.hpp

+//
+// Copyright (c) 2015 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_geometry_model_hpp__
+#define __se3_python_geometry_model_hpp__
+
+#include <boost/python/suite/indexing/vector_indexing_suite.hpp>
+#include <eigenpy/exception.hpp>
+#include <eigenpy/eigenpy.hpp>
+
+// #include "pinocchio/multibody/parser/sample-models.hpp"
+#include "pinocchio/python/se3.hpp"
+#include "pinocchio/python/eigen_container.hpp"
+#include "pinocchio/python/handler.hpp"
+
+#include "pinocchio/multibody/geometry.hpp"
+
+namespace se3
+{
+  namespace python
+  {
+    namespace bp = boost::python;
+
+    typedef Handler<GeometryModel> GeometryModelHandler;
+
+    struct GeometryModelPythonVisitor
+      : public boost::python::def_visitor< GeometryModelPythonVisitor >
+    {
+    public:
+      typedef GeometryModel::Index Index;
+      typedef eigenpy::UnalignedEquivalent<SE3>::type SE3_fx;
+      
+    public:
+
+      /* --- Convert From C++ to Python ------------------------------------- */
+      // static PyObject* convert(Model const& modelConstRef)
+      // {
+      // 	Model * ptr = const_cast<Model*>(&modelConstRef);
+      // 	return boost::python::incref(boost::python::object(ModelHandler(ptr)).ptr());
+      // }
+      static PyObject* convert(GeometryModelHandler::SmartPtr_t const& ptr)
+      {
+        return boost::python::incref(boost::python::object(GeometryModelHandler(ptr)).ptr());
+      }
+
+      /* --- Exposing C++ API to python through the handler ----------------- */
+    template<class PyClass>
+      void visit(PyClass& cl) const 
+      {
+	cl
+    .add_property("ngeom", &GeometryModelPythonVisitor::ngeom)
+
+    .def("getGeomId",&GeometryModelPythonVisitor::getGeomId)
+
+    .add_property("geometryPlacement",
+      bp::make_function(&GeometryModelPythonVisitor::geometryPlacement,
+            bp::return_internal_reference<>())  )
+    .add_property("geom_parents", 
+      bp::make_function(&GeometryModelPythonVisitor::geom_parents,
+            bp::return_internal_reference<>())  )
+    .add_property("geom_names",
+      bp::make_function(&GeometryModelPythonVisitor::geom_names,
+            bp::return_internal_reference<>())  )
+
+    .def("__str__",&GeometryModelPythonVisitor::toString)
+
+	  .def("BuildEmptyGeometryModel",&GeometryModelPythonVisitor::maker_empty)
+	  .staticmethod("BuildEmptyGeometryModel")
+	  ;
+      }
+
+      static GeometryModel::Index ngeom( GeometryModelHandler & m ) { return m->ngeom; }
+
+      static Model::Index getGeomId( const GeometryModelHandler & gmodelPtr, const std::string & name )
+      { return  gmodelPtr->getGeomId(name); }
+      
+      static std::vector<SE3> & geometryPlacement( GeometryModelHandler & m ) { return m->geometryPlacement; }
+      static std::vector<Model::Index> & geom_parents( GeometryModelHandler & m ) { return m->geom_parents; }
+      static std::vector<std::string> & geom_names ( GeometryModelHandler & m ) { return m->geom_names; }
+
+      
+
+      static GeometryModelHandler maker_empty()
+      {
+	return GeometryModelHandler( new GeometryModel(),true );
+      }
+ 
+
+      static std::string toString(const GeometryModelHandler& m) 
+      {	  std::ostringstream s; s << *m; return s.str();       }
+
+      /* --- Expose --------------------------------------------------------- */
+      static void expose()
+      {
+  
+  bp::class_<GeometryModelHandler>("GeometryModel",
+         "Geometry model (const)",
+         bp::no_init)
+    .def(GeometryModelPythonVisitor());
+      
+	/* Not sure if it is a good idea to enable automatic
+	 * conversion. Prevent it for now */
+	//bp::to_python_converter< Model,GeometryModelPythonVisitor >();
+	bp::to_python_converter< GeometryModelHandler::SmartPtr_t,GeometryModelPythonVisitor >();
+      }
+
+
+    };
+    
+
+
+  }} // namespace se3::python
+
+#endif // ifndef __se3_python_geometry_model_hpp__
+

src/python/python.cpp

@@ -30,6 +30,11 @@
 #include "pinocchio/python/parsers.hpp"
 #include "pinocchio/python/explog.hpp"
 
+#ifdef WITH_HPP_FCL
+#include "pinocchio/python/geometry-model.hpp"
+#include "pinocchio/python/geometry-data.hpp"
+#endif
+
 namespace se3
 {
   namespace python
@@ -62,6 +67,10 @@ namespace se3
     {
       ModelPythonVisitor::expose();
       DataPythonVisitor::expose();
+#ifdef WITH_HPP_FCL      
+      GeometryModelPythonVisitor::expose();
+      GeometryDataPythonVisitor::expose();
+#endif      
     }
     void exposeAlgorithms()
     {