[C++] Add const ref to model in GeometryModel

This reference is mandatory, just because a GeometryModel is linked to
a kinematic Model

src/multibody/geometry.hpp

@@ -166,6 +166,7 @@ namespace se3
     
     typedef std::list<GeomIndex> GeomIndexList;
 
+    const se3::Model & model;
     Index ngeom;
     std::vector<fcl::CollisionObject> collision_objects;
     std::vector<std::string> geom_names;
@@ -175,16 +176,16 @@ namespace se3
     std::map < JointIndex, GeomIndexList >  innerObjects;       // Associate a list of CollisionObjects to a given joint Id
     std::map < JointIndex, GeomIndexList >  outerObjects;       // Associate a list of CollisionObjects to a given joint Id
 
-    GeometryModel()
-      : ngeom(0)
+    GeometryModel(const se3::Model & model)
+      : model(model)
+      , ngeom(0)
       , collision_objects()
       , geom_names(0)
       , geom_parents(0)
       , geometryPlacement(0)
       , innerObjects()
       , outerObjects()
-    {
-    }
+    {}
 
     ~GeometryModel() {};
 

src/multibody/parser/urdf-with-geometry.hxx

@@ -103,7 +103,11 @@ namespace se3
     }
 
 
-    inline void parseTreeForGeom( ::urdf::LinkConstPtr link, Model & model,GeometryModel & model_geom, const std::string & meshRootDir, const bool rootJointAdded) throw (std::invalid_argument)
+    inline void parseTreeForGeom(::urdf::LinkConstPtr link,
+                                 Model & model,
+                                 GeometryModel & model_geom,
+                                 const std::string & meshRootDir,
+                                 const bool rootJointAdded) throw (std::invalid_argument)
     {
 
       // start with first link that is not empty
@@ -152,23 +156,37 @@ namespace se3
 
 
     template <typename D>
-    std::pair<Model, GeometryModel > buildModelAndGeom( const std::string & filename, const std::string & meshRootDir, const JointModelBase<D> &  root_joint )
+    std::pair<Model, GeometryModel>
+    buildModelAndGeom(const std::string & filename,
+                      const std::string & meshRootDir,
+                      const JointModelBase<D> & root_joint)
     {
-      Model model; GeometryModel model_geom;
-
+      // Read model
+      Model model;
       ::urdf::ModelInterfacePtr urdfTree = ::urdf::parseURDFFile (filename);
       parseTree(urdfTree->getRoot(), model, SE3::Identity(), root_joint);
+      
+      // Read geometries
+      GeometryModel model_geom(model);
       parseTreeForGeom(urdfTree->getRoot(), model, model_geom, meshRootDir, true);
+      
+      // Return a pair containing the kinematic tree and the geometries
       return std::make_pair(model, model_geom);
     }
 
-    inline std::pair<Model, GeometryModel > buildModelAndGeom( const std::string & filename, const std::string & meshRootDir)
+    inline std::pair<Model, GeometryModel> buildModelAndGeom(const std::string & filename,
+                                                              const std::string & meshRootDir)
     {
-      Model model; GeometryModel model_geom;
-
+      // Read model
+      Model model;
       ::urdf::ModelInterfacePtr urdfTree = ::urdf::parseURDFFile (filename);
       parseTree(urdfTree->getRoot(), model, SE3::Identity());
+      
+      // Read geometries
+      GeometryModel model_geom (model);
       parseTreeForGeom(urdfTree->getRoot(), model, model_geom, meshRootDir, false);
+      
+      // Return a pair containing the kinematic tree and the geometries
       return std::make_pair(model, model_geom);
     }
 

src/python/geometry-model.hpp

@@ -79,8 +79,8 @@ namespace se3
 
     .def("__str__",&GeometryModelPythonVisitor::toString)
 
-	  .def("BuildEmptyGeometryModel",&GeometryModelPythonVisitor::maker_empty)
-	  .staticmethod("BuildEmptyGeometryModel")
+	  .def("BuildGeometryModel",&GeometryModelPythonVisitor::maker_default)
+	  .staticmethod("BuildGeometryModel")
 	  ;
       }
 
@@ -95,9 +95,9 @@ namespace se3
 
       
 
-      static GeometryModelHandler maker_empty()
+      static GeometryModelHandler maker_default(const ModelHandler & model)
       {
-	return GeometryModelHandler( new GeometryModel(),true );
+        return GeometryModelHandler(new GeometryModel(*model), true);
       }
  
 

src/python/parsers.hpp

@@ -98,16 +98,13 @@ namespace se3
         template <typename JointModel>
         ModelGeometryHandlerPair_t operator() (const JointModel & root_joint) const
         {
-
-
-          Model * model = new Model();
-          GeometryModel * geometry_model = new GeometryModel();
           std::pair < Model, GeometryModel > models = se3::urdf::buildModelAndGeom(_filenameUrdf, _filenameMeshRootDir, root_joint);
-          *model = models.first;
-          *geometry_model = models.second;
-          return std::pair<ModelHandler, GeometryModelHandler> ( ModelHandler(model, true),
-                                                                 GeometryModelHandler(geometry_model, true)
-                                                               );
+          Model * model = new Model (models.first);
+          GeometryModel * geometry_model = new GeometryModel (models.second);
+          
+          return std::pair<ModelHandler, GeometryModelHandler> (ModelHandler(model, true),
+                                                                GeometryModelHandler(geometry_model, true)
+                                                                );
         }
       };
 
@@ -125,15 +122,15 @@ namespace se3
       buildModelAndGeomFromUrdf(const std::string & filename,
                                 const std::string & mesh_dir)
       {
-        Model * model = new Model();
-        GeometryModel * geometry_model = new GeometryModel();
         std::pair < Model, GeometryModel > models = se3::urdf::buildModelAndGeom(filename, mesh_dir);
-        *model = models.first;
-        *geometry_model = models.second;
-        return std::pair<ModelHandler, GeometryModelHandler> ( ModelHandler(model, true),
-                                                               GeometryModelHandler(geometry_model, true)
+        Model * model = new Model(models.first);
+        GeometryModel * geometry_model = new GeometryModel(models.second);
+        
+        return std::pair<ModelHandler, GeometryModelHandler> (ModelHandler(model, true),
+                                                              GeometryModelHandler(geometry_model, true)
                                                              );
       }
+      
 #endif // #ifdef WITH_HPP_FCL
 #endif // #ifdef WITH_URDFDOM
 

unittest/geom.cpp

@@ -145,7 +145,7 @@ BOOST_AUTO_TEST_SUITE ( GeomTest )
 BOOST_AUTO_TEST_CASE ( simple_boxes )
 {
   se3::Model model;
-  se3::GeometryModel model_geom;
+  se3::GeometryModel model_geom(model);
 
   se3::buildModels::collisionModel(model, model_geom);