[Python][Bug Fixed] Properly handle geometry data

src/python/geometry-data.hpp

@@ -25,8 +25,8 @@
 #include "pinocchio/python/se3.hpp"
 #include "pinocchio/python/eigen_container.hpp"
 #include "pinocchio/python/handler.hpp"
-
-#include "pinocchio/multibody/geometry.hpp"
+#include "pinocchio/python/data.hpp"
+#include "pinocchio/python/geometry-model.hpp"
 
 namespace se3
 {
@@ -62,64 +62,117 @@ namespace se3
     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<>())  )
-    .add_property("collisions",
-      bp::make_function(&GeometryDataPythonVisitor::collisions,
-            bp::return_internal_reference<>())  )
-
-    .def("addCollisionPair",&GeometryDataPythonVisitor::isCollisionPair)
-    .def("removeCollisionPair",&GeometryDataPythonVisitor::isCollisionPair)
-    .def("isCollisionPair",&GeometryDataPythonVisitor::isCollisionPair)
-    .def("collide",&GeometryDataPythonVisitor::collide)
-
-    .def("__str__",&GeometryDataPythonVisitor::toString)
-
-
+        cl
+        .def("__init__",
+             bp::make_constructor(&GeometryDataPythonVisitor::makeDefault,
+                                  bp::default_call_policies(),
+                                  (bp::arg("data"),bp::arg("geometry_model"))),
+             "Initialize from data and the geometry model.")
+        
+        .add_property("nCollisionPairs", &GeometryDataPythonVisitor::nCollisionPairs)
+        
+        .add_property("oMg",
+                      bp::make_function(&GeometryDataPythonVisitor::oMg,
+                                        bp::return_internal_reference<>()),
+                      "Vector of collision objects placement relative to the world.")
+        .add_property("collision_pairs",
+                      bp::make_function(&GeometryDataPythonVisitor::collision_pairs,
+                                        bp::return_internal_reference<>()),
+                      "Vector of collision pairs.")
+        .add_property("distances",
+                      bp::make_function(&GeometryDataPythonVisitor::distances,
+                                        bp::return_internal_reference<>()),
+                      "Vector of distance results computed in ")
+        .add_property("collisions",
+                      bp::make_function(&GeometryDataPythonVisitor::collisions,
+                                        bp::return_internal_reference<>())  )
+        
+        .def("addCollisionPair",&GeometryDataPythonVisitor::addCollisionPair,
+             bp::args("co1 (index)","co2 (index)"),
+             "Add a collision pair given by the index of the two collision objects."
+             " Remark: co1 < co2")
+        .def("addAllCollisionPairs",&GeometryDataPythonVisitor::addAllCollisionPairs,
+             "Add all collision pairs.")
+        .def("removeCollisionPair",&GeometryDataPythonVisitor::removeCollisionPair,
+             bp::args("co1 (index)","co2 (index)"),
+             "Remove a collision pair given by the index of the two collision objects."
+             " Remark: co1 < co2")
+        .def("removeAllCollisionPairs",&GeometryDataPythonVisitor::removeAllCollisionPairs,
+             "Remove all collision pairs.")
+        .def("existCollisionPair",&GeometryDataPythonVisitor::existCollisionPair,
+             bp::args("co1 (index)","co2 (index)"),
+             "Check if a collision pair given by the index of the two collision objects exists or not."
+             " Remark: co1 < co2")
+        .def("findCollisionPair", &GeometryDataPythonVisitor::findCollisionPair,
+             bp::args("co1 (index)","co2 (index)"),
+             "Return the index of a collision pair given by the index of the two collision objects exists or not."
+             " Remark: co1 < co2")
+        .def("collide",&GeometryDataPythonVisitor::collide,
+             bp::args("co1 (index)","co2 (index)"),
+             "Check if the two collision objects of a collision pair are in collision."
+             "The collision pair is given by the two index of the collision objects.")
+        
+        .def("__str__",&GeometryDataPythonVisitor::toString)
 	  ;
       }
+      
+      static GeometryDataHandler* makeDefault(const DataHandler & data, const GeometryModelHandler & geometry_model)
+      {
+        return new GeometryDataHandler(new GeometryData(*data, *geometry_model), true);
+      }
 
-      static GeometryModel::Index nCollisionPairs( GeometryDataHandler & m ) { return m->nCollisionPairs; }
+      static Index nCollisionPairs(const GeometryDataHandler & m ) { return m->nCollisionPairs; }
       
-      static std::vector<SE3> & oMg( GeometryDataHandler & m ) { return m->oMg; }
+      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 std::vector<bool> & collisions( GeometryDataHandler & m ) { return m->collisions; }
 
-      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 void addCollisionPair (GeometryDataHandler & m, const Index co1, const Index co2)
+      {
+        m->addCollisionPair(co1, co2);
+      }
+      static void addAllCollisionPairs (GeometryDataHandler & m)
+      {
+        m->addAllCollisionPairs();
+      }
       
- 
+      static void removeCollisionPair (GeometryDataHandler & m, const Index co1, const Index co2)
+      {
+        m->removeCollisionPair(co1, co2);
+      }
+      static void removeAllCollisionPairs (GeometryDataHandler & m)
+      {
+        m->removeAllCollisionPairs();
+      }
+      
+      static bool existCollisionPair (const GeometryDataHandler & m, const Index co1, const Index co2)
+      {
+        return m->existCollisionPair(co1, co2);
+      }
+      static GeometryData::Index findCollisionPair (const GeometryDataHandler & m, const Index co1, const Index co2)
+      {
+        return m->findCollisionPair(co1, co2);
+      }
 
-      static std::string toString(const GeometryDataHandler& m) 
+      static bool collide(const GeometryDataHandler & m, const Index co1, const Index co2) { return m -> collide(co1, co2); };
+      
+      static std::string toString(const GeometryDataHandler& m)
       {	  std::ostringstream s; s << *m; return s.str();       }
 
       /* --- Expose --------------------------------------------------------- */
       static void expose()
       {
+        
+        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> >());
   
-  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> >());
+        bp::class_<GeometryDataHandler>("GeometryData",
+                                 "Geometry data linked to a geometry model",
+                                 bp::no_init)
+        .def(GeometryDataPythonVisitor());
      
 	bp::to_python_converter< GeometryDataHandler::SmartPtr_t,GeometryDataPythonVisitor >();
       }