diff --git a/src/multibody/parser/urdf-with-geometry.hpp b/src/multibody/parser/urdf-with-geometry.hpp
index a5a26032f596298144a804c1ec383e6234c39b23..13283359354773d326f2a2ce5f870d015ae52003 100644
--- a/src/multibody/parser/urdf-with-geometry.hpp
+++ b/src/multibody/parser/urdf-with-geometry.hpp
@@ -56,7 +56,8 @@ namespace se3
      *
      * @return     The mesh converted as a fcl::CollisionObject
      */
-    inline fcl::CollisionObject retrieveCollisionGeometry (const ::urdf::LinkConstPtr & link, const std::string & meshRootDir);
+    inline fcl::CollisionObject retrieveCollisionGeometry (const ::urdf::LinkConstPtr & link,
+                                                           const std::string & meshRootDir);
 
     
     /**
diff --git a/src/multibody/parser/urdf-with-geometry.hxx b/src/multibody/parser/urdf-with-geometry.hxx
index 720ed585156bfad39db33601592831a1c19c3a66..d639299eae41e5440e90fcbdeb9a4bd06f8a8ceb 100644
--- a/src/multibody/parser/urdf-with-geometry.hxx
+++ b/src/multibody/parser/urdf-with-geometry.hxx
@@ -37,7 +37,8 @@ namespace se3
   namespace urdf
   {
     
-    inline fcl::CollisionObject retrieveCollisionGeometry (const ::urdf::LinkConstPtr & link, const std::string & meshRootDir)
+    inline fcl::CollisionObject retrieveCollisionGeometry (const ::urdf::LinkConstPtr & link,
+                                                           const std::string & meshRootDir)
     {
       boost::shared_ptr < ::urdf::Collision> collision = link->collision;
       boost::shared_ptr < fcl::CollisionGeometry > geometry;
@@ -104,7 +105,7 @@ namespace se3
 
 
     inline void parseTreeForGeom(::urdf::LinkConstPtr link,
-                                 Model & model,
+                                 const Model & model,
                                  GeometryModel & model_geom,
                                  const std::string & meshRootDir,
                                  const bool rootJointAdded) throw (std::invalid_argument)
@@ -121,8 +122,8 @@ namespace se3
           if (link->collision)
           {
             fcl::CollisionObject collision_object = retrieveCollisionGeometry(link, meshRootDir);
-            SE3 geomPlacement = convertFromUrdf(link->collision->origin);
-            std::string collision_object_name = link->name ;
+            const SE3 geomPlacement = convertFromUrdf(link->collision->origin);
+            const std::string & collision_object_name = link->name ;
             model_geom.addGeomObject(model.getJointId("root_joint"), collision_object, geomPlacement, collision_object_name);
           }
         }
@@ -134,8 +135,8 @@ namespace se3
           if (link->collision)
           {
             fcl::CollisionObject collision_object = retrieveCollisionGeometry(link, meshRootDir);
-            SE3 geomPlacement = convertFromUrdf(link->collision->origin);
-            std::string collision_object_name = link->name ;
+            const SE3 geomPlacement = convertFromUrdf(link->collision->origin);
+            const std::string & collision_object_name = link->name ;
             model_geom.addGeomObject(model.getJointId(joint->name), collision_object, geomPlacement, collision_object_name);
           }      
         }