Merge pull request #110 from fvalenza/topic/parse-ros-path

Recreate resource retriever behaviour by specifying the ROS_PACKAGE_PATH as environment variable.

CMakeLists.txt

@@ -86,6 +86,7 @@ SET(${PROJECT_NAME}_MATH_HEADERS
 SET(${PROJECT_NAME}_TOOLS_HEADERS
   tools/timer.hpp
   tools/string-generator.hpp
+  tools/file-explorer.hpp
   )
 
 SET(${PROJECT_NAME}_SPATIAL_HEADERS

benchmark/timings-geometry.cpp

@@ -69,11 +69,15 @@ int main()
 
 
   std::string romeo_filename = PINOCCHIO_SOURCE_DIR"/models/romeo.urdf";
+  std::vector < std::string > package_dirs;
   std::string romeo_meshDir  = PINOCCHIO_SOURCE_DIR"/models/";
-  Model model = se3::urdf::buildModel(romeo_filename, se3::JointModelFreeFlyer());
-  GeometryModel geom_model = se3::urdf::buildGeom(model, romeo_filename, romeo_meshDir);
+  package_dirs.push_back(romeo_meshDir);
+
+  se3::Model model = se3::urdf::buildModel(romeo_filename, se3::JointModelFreeFlyer());
+  se3::GeometryModel geom_model = se3::urdf::buildGeom(model, romeo_filename, package_dirs);
+   
   Data data(model);
-  GeometryData geom_data (data, geom_model);
+  GeometryData geom_data(data, geom_model);
   geom_data.initializeListOfCollisionPairs();
 
 

src/multibody/parser/from-collada-to-fcl.hpp

@@ -35,6 +35,8 @@
 #include <hpp/fcl/BV/OBBRSS.h>
 #include <hpp/fcl/BVH/BVH_model.h>
 
+#include "pinocchio/tools/file-explorer.hpp"
+
 #include <exception>
 
 namespace se3
@@ -196,22 +198,35 @@ namespace se3
   
   
   /**
-   * @brief      Transform a cURL readable path (package://..) to an absolute path for urdf collision path
+   * @brief      Transform a package://.. mesh path to an absolute path, searching for a valid file 
+   *             in a list of package directories
    *
-   * @param[in]      urdf_mesh_path  The path given in the urdf file (package://..)
-   * @param[in]  meshRootDir     Root path to the directory where meshes are located
+   * @param[in]  urdf_mesh_path  The path given in the urdf file
+   * @param[in]  package_dirs    A list of packages directories where to search for meshes
    *
    * @return     The absolute path to the mesh file
    */
-  inline std::string fromURDFMeshPathToAbsolutePath(const std::string & urdf_mesh_path,
-                                                    const std::string & meshRootDir)
-  {
-    
-    std::string absolutePath = std::string(meshRootDir +
-                                           urdf_mesh_path.substr(10, urdf_mesh_path.size()));
-    
+   inline std::string convertURDFMeshPathToAbsolutePath(const std::string & urdf_mesh_path,
+                                                         const std::vector<std::string> & package_dirs)
+   {
+    // if exists p1/mesh, absolutePath = p1/mesh,
+    // else if exists p2/mesh, absolutePath = p2/mesh
+    // else return an empty string that will provoke an error in loadPolyhedronFromResource()
+    namespace bf = boost::filesystem;
+
+    std::string absolutePath;
+    // concatenate package_path with mesh filename
+    for (int i = 0; i < package_dirs.size(); ++i)
+    {
+      if ( bf::exists( bf::path(package_dirs[i] +  urdf_mesh_path.substr(9, urdf_mesh_path.size()))))
+      {
+          absolutePath = std::string( package_dirs[i] + urdf_mesh_path.substr(9, urdf_mesh_path.size())
+                                       );
+        break;
+      }
+    }
     return absolutePath;
-  }
+   }
 
 } // namespace se3
 

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

@@ -35,47 +35,53 @@ namespace se3
 
 
     /**
-     * @brief      Get a CollisionObject from an urdf link, reading the
-     *             corresponding mesh
+     * @brief      Get a CollisionObject from an urdf link, searching
+     *             for it in specified package directories
      *
-     * @param[in]  link         The input urdf link
-     * @param[in]  meshRootDir  Root path to the directory where meshes are located
+     * @param[in]  link          The input urdf link
+     * @param[in]  package_dirs  A vector containing the different directories where to search for packages
      *
      * @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::vector < std::string > & package_dirs);
 
     
     /**
      * @brief      Recursive procedure for reading the URDF tree, looking for geometries
      *             This function fill the geometric model whith geometry objects retrieved from the URDF tree
-     *
-     * @param[in]  link         The current URDF link
-     * @param      model        The model to which is the Geometry Model associated
-     * @param      model_geom   The Geometry Model where the Collision Objects must be added
-     * @param[in]  meshRootDir  Root path to the directory where meshes are located
+     * 
+     * @param[in]  link            The current URDF link
+     * @param      model           The model to which is the GeometryModel associated
+     * @param      model_geom      The GeometryModel where the Collision Objects must be added
+     * @param[in]  package_dirs    A vector containing the different directories where to search for packages
+     * @param[in]  rootJointAdded  If a root joint was added at the begining of the urdf kinematic chain by user when constructing the Model
      */
-    inline void parseTreeForGeom(::urdf::LinkConstPtr link,
-                                 const Model & model,
-                                 GeometryModel & model_geom,
-                                 const std::string & meshRootDir,
-                                 const bool rootJointAdded) throw (std::invalid_argument);
+    inline void parseTreeForGeom( ::urdf::LinkConstPtr link,
+                                const Model & model,
+                                GeometryModel & model_geom,
+                                const std::vector<std::string> & package_dirs) throw (std::invalid_argument);
 
 
 
     /**
-     * @brief      Build the GeometryModel from a URDF file.
+     * @brief      Build The GeometryModel from a URDF file. Search for meshes
+     *             in the directories specified by the user first and then in
+     *             the environment variable ROS_PACKAGE_PATH
      *
-     * @param[in]  model     The model of the robot, built with urdf::buildModel.
-     * @param[in]  filename     The complete path to the URDF model.
-     * @param[in]  meshRootDir  Root path pointing to the directory where meshes are located.
+     * @param      model         The model of the robot, built with
+     *                           urdf::buildModel
+     * @param[in]  filename      The URDF complete (absolute) file path
+     * @param[in]  package_dirs  A vector containing the different directories
+     *                           where to search for packages
+     * @param[in]  root_joint    If we added a root joint to the Model in
+     *                           addition to the urdf kinematic chain
      *
-     * @return     The geometric model.
+     * @return     The GeometryModel associated to the urdf file and the Model
+     *             given
      */
-    GeometryModel buildGeom(const Model & model,
-                            const std::string & filename,
-                            const std::string & meshRootDir);
+    inline GeometryModel buildGeom(const Model & model,
+                                  const std::string & filename,
+                                  const std::vector<std::string> & package_dirs = std::vector<std::string>());
 
   } // namespace urdf
   

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

@@ -31,8 +31,7 @@ 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::vector < std::string > & package_dirs)
     {
       boost::shared_ptr < ::urdf::Collision> collision = link->collision;
       boost::shared_ptr < fcl::CollisionGeometry > geometry;
@@ -43,7 +42,7 @@ namespace se3
         boost::shared_ptr < ::urdf::Mesh> collisionGeometry = boost::dynamic_pointer_cast< ::urdf::Mesh> (collision->geometry);
         std::string collisionFilename = collisionGeometry->filename;
 
-        std::string full_path = fromURDFMeshPathToAbsolutePath(collisionFilename, meshRootDir);
+        std::string full_path = convertURDFMeshPathToAbsolutePath(collisionFilename, package_dirs);
 
         ::urdf::Vector3 scale = collisionGeometry->scale;
 
@@ -101,68 +100,58 @@ namespace se3
     inline void parseTreeForGeom(::urdf::LinkConstPtr link,
                                  const Model & model,
                                  GeometryModel & geom_model,
-                                 const std::string & meshRootDir,
-                                 const bool rootJointAdded) throw (std::invalid_argument)
+                                 const std::vector<std::string> & package_dirs) throw (std::invalid_argument)
     {
 
       // start with first link that is not empty
       if(link->collision)
       {
-        ::urdf::JointConstPtr joint = link->parent_joint;
 
-        if (joint == NULL && rootJointAdded)
-        {
-            fcl::CollisionObject collision_object = retrieveCollisionGeometry(link, meshRootDir);
-            const SE3 geomPlacement = convertFromUrdf(link->collision->origin);
-            const std::string & collision_object_name = link->name ;
-            geom_model.addGeomObject(model.getJointId("root_joint"), collision_object, geomPlacement, collision_object_name);
-          
-        }
-        else if(joint!=NULL)
-        {
-          assert(link->getParent()!=NULL);
 
-          fcl::CollisionObject collision_object = retrieveCollisionGeometry(link, meshRootDir);
-          const SE3 geomPlacement = convertFromUrdf(link->collision->origin);
-          const std::string & collision_object_name = link->name ;
-          geom_model.addGeomObject(model.getJointId(joint->name), collision_object, geomPlacement, collision_object_name);
-        }
-        else if (link->getParent() != NULL)
+        assert(link->getParent()!=NULL);
+
+        fcl::CollisionObject collision_object = retrieveCollisionGeometry(link, package_dirs);
+        SE3 geomPlacement = convertFromUrdf(link->collision->origin);
+        std::string collision_object_name = link->name ;
+        geom_model.addGeomObject(model.parents[model.getBodyId(collision_object_name)], collision_object, geomPlacement, collision_object_name);     
+
+        if (link->getParent() == NULL)
         {
           const std::string exception_message (link->name + " - joint information missing.");
           throw std::invalid_argument(exception_message);
         }
 
-      } // if(link->inertial)
+      } // if(link->collision)
       
       BOOST_FOREACH(::urdf::LinkConstPtr child,link->child_links)
       {
-        parseTreeForGeom(child, model, geom_model, meshRootDir, rootJointAdded);
+        parseTreeForGeom(child, model, geom_model, package_dirs);
       }
+
     }
 
-    GeometryModel buildGeom(const Model & model,
-                            const std::string & filename,
-                            const std::string & meshRootDir)
+
+    inline GeometryModel buildGeom(const Model & model,
+                                  const std::string & filename,
+                                  const std::vector<std::string> & package_dirs)
     {
-      ::urdf::ModelInterfacePtr urdfTree = ::urdf::parseURDFFile (filename);
-      ::urdf::LinkConstPtr root_link = urdfTree->getRoot();
-      if (!root_link->inertial) // if the first body is just a base_link, i.e. with no inertial info
+      GeometryModel model_geom(model);
+
+      std::vector<std::string> hint_directories(package_dirs);
+
+      appendRosPackagePaths(hint_directories);
+
+      if(hint_directories.empty())
       {
-        ::urdf::LinkPtr child_link = root_link->child_links[0];
-        
-        // Change the name of the parent joint
-        child_link->parent_joint->name = "root_joint";
+        throw std::runtime_error("You did not specify any package directory and ROS_PACKAGE_PATH is empty. Geometric parsing will crash");
       }
-      
-      // Read geometries
-      GeometryModel geom_model (model);
-      parseTreeForGeom(urdfTree->getRoot(), model, geom_model, meshRootDir, true);
-      
-      // Return a pair containing the kinematic tree and the geometries
-      return geom_model;
+
+      ::urdf::ModelInterfacePtr urdfTree = ::urdf::parseURDFFile (filename);
+      parseTreeForGeom(urdfTree->getRoot(), model, model_geom, hint_directories);
+      return model_geom;
     }
 
+
   } // namespace urdf
 } // namespace se3
 

src/python/parsers.hpp

@@ -53,11 +53,11 @@ namespace se3
       
       
 #ifdef WITH_URDFDOM
-      struct build_model_visitor : public boost::static_visitor<ModelHandler>
+      struct BuildModelVisitor : public boost::static_visitor<ModelHandler>
       {
         const std::string& _filename;
 
-        build_model_visitor(const std::string& filename): _filename(filename){}
+        BuildModelVisitor(const std::string& filename): _filename(filename){}
 
         template <typename JointModel> ModelHandler operator()(const JointModel & root_joint) const
         {
@@ -72,7 +72,7 @@ namespace se3
                                              )
       {
         JointModelVariant root_joint = bp::extract<JointModelVariant> (root_joint_object);
-        return boost::apply_visitor(build_model_visitor(filename), root_joint);
+        return boost::apply_visitor(BuildModelVisitor(filename), root_joint);
       }
 
       static ModelHandler buildModelFromUrdf(const std::string & filename)
@@ -85,49 +85,26 @@ namespace se3
 
 #ifdef WITH_HPP_FCL
       typedef std::pair<ModelHandler, GeometryModelHandler> ModelGeometryHandlerPair_t;
-      struct build_model_and_geom_visitor : public boost::static_visitor<std::pair<ModelHandler, GeometryModelHandler> >
-      {
-        const std::string& _filenameUrdf;
-        const std::string& _filenameMeshRootDir;
-
-        build_model_and_geom_visitor(const std::string & filenameUrdf,
-                                     const std::string & filenameMeshRootDir)
-          : _filenameUrdf(filenameUrdf)
-          , _filenameMeshRootDir(filenameMeshRootDir)
-        {}
-
-        template <typename JointModel>
-        ModelGeometryHandlerPair_t operator() (const JointModel & root_joint) const
-        {
-          Model * model = new Model(se3::urdf::buildModel(_filenameUrdf, root_joint));
-          GeometryModel * geometry_model = new GeometryModel (se3::urdf::buildGeom(*model, _filenameUrdf, _filenameMeshRootDir));
-          
-          return std::pair<ModelHandler, GeometryModelHandler> (ModelHandler(model, true),
-                                                                GeometryModelHandler(geometry_model, true)
-                                                                );
-        }
-      };
-
-      static ModelGeometryHandlerPair_t
-      buildModelAndGeomFromUrdf(const std::string & filename,
-                                const std::string & mesh_dir,
-                                bp::object & root_joint_object
-                                )
+      
+      static GeometryModelHandler
+      buildGeomFromUrdf(const ModelHandler & model,
+                        const std::string & filename
+                        )
       {
-        JointModelVariant root_joint = bp::extract<JointModelVariant> (root_joint_object);
-        return boost::apply_visitor(build_model_and_geom_visitor(filename, mesh_dir), root_joint);
+        GeometryModel * geometry_model = new GeometryModel(se3::urdf::buildGeom(*model, filename));
+        
+        return GeometryModelHandler(geometry_model, true);
       }
 
-      static ModelGeometryHandlerPair_t
-      buildModelAndGeomFromUrdf(const std::string & filename,
-                                const std::string & mesh_dir)
+      static GeometryModelHandler
+      buildGeomFromUrdf(const ModelHandler & model,
+                        const std::string & filename,
+                        std::vector<std::string> & package_dirs
+                        )
       {
-        Model * model = new Model(se3::urdf::buildModel(filename));
-        GeometryModel * geometry_model = new GeometryModel(se3::urdf::buildGeom(*model, filename, mesh_dir));
+        GeometryModel * geometry_model = new GeometryModel(se3::urdf::buildGeom(*model, filename, package_dirs));
         
-        return ModelGeometryHandlerPair_t (ModelHandler(model, true),
-                                           GeometryModelHandler(geometry_model, true)
-                                           );
+        return GeometryModelHandler(geometry_model, true);
       }
       
 #endif // #ifdef WITH_HPP_FCL
@@ -172,17 +149,17 @@ namespace se3
       
       bp::to_python_converter<std::pair<ModelHandler, GeometryModelHandler>, PairToTupleConverter<ModelHandler, GeometryModelHandler> >();
       
-      bp::def("buildModelAndGeomFromUrdf",
-              static_cast <ModelGeometryHandlerPair_t (*) (const std::string &, const std::string &, bp::object &)> (&ParsersPythonVisitor::buildModelAndGeomFromUrdf),
-              bp::args("filename (string)", "mesh_dir (string)",
-                       "Root Joint Model"),
-              "Parse the urdf file given in input and return a proper pinocchio model starting with a given root joint and geometry model "
+      bp::def("buildGeomFromUrdf",
+              static_cast <GeometryModelHandler (*) (const ModelHandler &, const std::string &, std::vector<std::string> &)> (&ParsersPythonVisitor::buildGeomFromUrdf),
+              bp::args("Model to assosiate the Geometry","filename (string)", "package_dirs (vector of strings)"
+                       ),
+              "Parse the urdf file given in input looking for the geometry of the given Model and return a proper pinocchio geometry model "
               "(remember to create the corresponding data structures).");
       
-      bp::def("buildModelAndGeomFromUrdf",
-              static_cast <ModelGeometryHandlerPair_t (*) (const std::string &, const std::string &)> (&ParsersPythonVisitor::buildModelAndGeomFromUrdf),
-              bp::args("filename (string)", "mesh_dir (string)"),
-              "Parse the urdf file given in input and return a proper pinocchio model and geometry model "
+      bp::def("buildGeomFromUrdf",
+              static_cast <GeometryModelHandler (*) (const ModelHandler &, const std::string &)> (&ParsersPythonVisitor::buildGeomFromUrdf),
+              bp::args("Model to assosiate the Geometry","filename (string)"),
+              "Parse the urdf file given in input looking for the geometry of the given Model and return a proper pinocchio  geometry model "
               "(remember to create the corresponding data structures).");
       
 #endif // #ifdef WITH_HPP_FCL

src/python/robot_wrapper.py

@@ -22,32 +22,26 @@ import time
 
 class RobotWrapper:
 
-    def __init__(self, filename, mesh_dir = None, root_joint = None):
-        if isinstance(mesh_dir, basestring):
-          build_model_and_geom = True
-        else: # Only load the model
-          build_model_and_geom = False
-          root_joint = mesh_dir
-      
-        self.model_filename = filename
-        if build_model_and_geom:
-          # Check if the module geometry of Pinocchio has been compiled
-          if not "buildModelAndGeomFromUrdf" in dir(se3):
-            raise Exception('It seems that the Geometry Module has not been compiled with Pinocchio') 
-          if(root_joint is None):
-              self.model, self.geometry_model = se3.buildModelAndGeomFromUrdf(filename,mesh_dir)
-          else:
-              self.model, self.geometry_model = se3.buildModelAndGeomFromUrdf(filename,mesh_dir,root_joint)
-
-          self.data = self.model.createData()
-          self.geometry_data = se3.GeometryData(self.data, self.geometry_model)
-        else:
-          if(root_joint is None):
+    def __init__(self, filename, package_dirs = None, root_joint = None):
+        if(root_joint is None):
               self.model = se3.buildModelFromUrdf(filename)
-          else:
+        else:
               self.model = se3.buildModelFromUrdf(filename, root_joint)
-          
-          self.data = self.model.createData()
+
+        self.data = self.model.createData()
+
+        if not "buildGeomFromUrdf" in dir(se3):
+            raise Exception('It seems that the Geometry Module has not been compiled with Pinocchio. No geometry model')
+        else:
+            if (package_dirs is None):
+                self.geometry_model = se3.buildGeomFromUrdf(self.model, filename)
+                self.geometry_data = se3.GeometryData(self.data, self.geometry_model)
+            else:
+                if not all(isinstance(item,basestring) for item in package_dirs):
+                    raise Exception('The list of package directories is wrong. At least one is not a string')
+                else:
+                    self.geometry_model = se3.buildGeomFromUrdf(self.model, filename, utils.fromListToVectorOfString(package_dirs))
+                    self.geometry_data = se3.GeometryData(self.data, self.geometry_model)
 
         self.v0 = utils.zero(self.nv)
         self.q0 = utils.zero(self.nq)

src/python/utils.py

@@ -104,6 +104,11 @@ def mprint(M,name = "ans"):
             print ""
         print ""
 
+def fromListToVectorOfString(list):
+    vector = se3.StdVec_StdString()
+    vector.extend(item for item in list)
+    return vector
+
 
 from rpy import *
 
@@ -112,4 +117,4 @@ __all__ = [ 'np','npl','eye','zero','rand','isapprox','mprint',
             'npToTTuple','npToTuple','rotate',
             'rpyToMatrix','matrixToRpy',
             'se3ToXYZQUAT' ,'XYZQUATToSe3',
-            'XYZQUATToViewerConfiguration', 'ViewerConfigurationToXYZQUAT' ]
+            'XYZQUATToViewerConfiguration', 'ViewerConfigurationToXYZQUAT', 'fromListToVectorOfString' ]

src/tools/file-explorer.hpp

+//
+// Copyright (c) 2016 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_file_explorer_hpp__
+#define __se3_file_explorer_hpp__
+
+#include <string>
+#include <iostream>
+#include <vector>
+#include <exception>
+
+#include "boost/filesystem.hpp"
+
+namespace se3
+{
+
+    /**
+     * @brief      Parse env variable ROS_PACKAGE_PATH to extract paths and append them
+     *              in the package_dirs variable
+     *
+     * @param[in][out]  paths  { The package directories where to search for meshes }
+     */
+    inline void appendRosPackagePaths(std::vector<std::string> & package_dirs)
+    {
+
+        std::string delimiter = ":";
+        std::string policyStr = std::getenv("ROS_PACKAGE_PATH");
+        size_t lastOffset = 0;
+
+        while(true)
+        {
+            size_t offset = policyStr.find_first_of(delimiter, lastOffset);
+            if (offset < policyStr.size())
+              package_dirs.push_back(policyStr.substr(lastOffset, offset - lastOffset));
+            if (offset == std::string::npos)
+                break;
+            else
+                lastOffset = offset + 1; // add one to skip the delimiter
+        }
+
+    }
+
+}
+
+#endif // __se3_file_explorer_hpp__

unittest/geom.cpp

@@ -205,9 +205,12 @@ BOOST_AUTO_TEST_CASE ( loading_model )
 
 
   std::string filename = PINOCCHIO_SOURCE_DIR"/models/romeo.urdf";
+  std::vector < std::string > package_dirs;
   std::string meshDir  = PINOCCHIO_SOURCE_DIR"/models/";
+  package_dirs.push_back(meshDir);
+
   Model model = se3::urdf::buildModel(filename, se3::JointModelFreeFlyer());
-  GeometryModel geometry_model = se3::urdf::buildGeom(model, filename, meshDir);
+  GeometryModel geometry_model = se3::urdf::buildGeom(model, filename, package_dirs);
 
   Data data(model);
   GeometryData geometry_data(data, geometry_model);
@@ -243,16 +246,22 @@ BOOST_AUTO_TEST_CASE ( romeo_joints_meshes_positions )
 
   // Building the model in pinocchio and compute kinematics/geometry for configuration q_pino
   std::string filename = PINOCCHIO_SOURCE_DIR"/models/romeo.urdf";
+  std::vector < std::string > package_dirs;
   std::string meshDir  = PINOCCHIO_SOURCE_DIR"/models/";
-  std::pair < Model, GeometryModel > robot = se3::urdf::buildModelAndGeom(filename, meshDir, se3::JointModelFreeFlyer());
+  package_dirs.push_back(meshDir);
+
+  se3::Model model = se3::urdf::buildModel(filename, se3::JointModelFreeFlyer());
+  se3::GeometryModel geom = se3::urdf::buildGeom(model, filename, package_dirs);
+  std::cout << model << std::endl;
 
-  Data data(robot.first);
-  GeometryData data_geom(data, robot.second);
+
+  Data data(model);
+  GeometryData data_geom(data, geom);
 
   // Configuration to be tested
   
 
-  Eigen::VectorXd q_pino(Eigen::VectorXd::Random(robot.first.nq));
+  Eigen::VectorXd q_pino(Eigen::VectorXd::Random(model.nq));
   q_pino.segment<4>(3) /= q_pino.segment<4>(3).norm();
 
   Eigen::VectorXd q_hpp(q_pino);
@@ -261,9 +270,9 @@ BOOST_AUTO_TEST_CASE ( romeo_joints_meshes_positions )
   q_hpp.segment<4>(3) = quaternion ;
 
 
-  assert(q_pino.size() == robot.first.nq && "wrong config size");
+  assert(q_pino.size() == model.nq && "wrong config size");
 
-  se3::updateGeometryPlacements(robot.first, data, robot.second, data_geom, q_pino);
+  se3::updateGeometryPlacements(model, data, geom, data_geom, q_pino);
 
 
   /// *************  HPP  ************* /// 
@@ -277,7 +286,7 @@ BOOST_AUTO_TEST_CASE ( romeo_joints_meshes_positions )
               "", "");
 
 
-  assert(robot.first.nq == humanoidRobot->configSize () && "Pinocchio model & HPP model config sizes are not the same ");
+  assert(model.nq == humanoidRobot->configSize () && "Pinocchio model & HPP model config sizes are not the same ");
 
   humanoidRobot->currentConfiguration (q_hpp);
   humanoidRobot->computeForwardKinematics ();
@@ -347,16 +356,22 @@ BOOST_AUTO_TEST_CASE ( hrp2_mesh_distance)
 
   // Building the model in pinocchio and compute kinematics/geometry for configuration q_pino
   std::string filename = PINOCCHIO_SOURCE_DIR"/models/romeo.urdf";
+  std::vector < std::string > package_dirs;
   std::string meshDir  = PINOCCHIO_SOURCE_DIR"/models/";
-  std::pair < Model, GeometryModel > robot = se3::urdf::buildModelAndGeom(filename, meshDir, se3::JointModelFreeFlyer());
+  package_dirs.push_back(meshDir);
 
-  Data data(robot.first);
-  GeometryData data_geom(data, robot.second);