diff --git a/src/python/parsers.hpp b/src/python/parsers.hpp
index 2894c989a0dd271272dbc88778a426a841953c16..4f51bbab79af7eff9c34a093c5ab3ae0a277dabe 100644
--- a/src/python/parsers.hpp
+++ b/src/python/parsers.hpp
@@ -85,49 +85,28 @@ namespace se3
#ifdef WITH_HPP_FCL
typedef std::pair<ModelHandler, GeometryModelHandler> ModelGeometryHandlerPair_t;
- struct BuildModelAndGeomVisitor : public boost::static_visitor<std::pair<ModelHandler, GeometryModelHandler> >
- {
- const std::string& _filenameUrdf;
- const std::vector<std::string> & _package_dirs;
-
- BuildModelAndGeomVisitor(const std::string & filenameUrdf,
- const std::vector<std::string> & package_dirs)
- : _filenameUrdf(filenameUrdf)
- , _package_dirs(package_dirs)
- {}
-
- 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, _package_dirs));
-
- return std::pair<ModelHandler, GeometryModelHandler> (ModelHandler(model, true),
- GeometryModelHandler(geometry_model, true)
- );
- }
- };
-
- static ModelGeometryHandlerPair_t
- buildModelAndGeomFromUrdf(const std::string & filename,
- const std::vector<std::string> & package_dirs,
- bp::object & root_joint_object
- )
+
+ static GeometryModelHandler
+ buildGeomFromUrdf(const ModelHandler & model,
+ const std::string & filename,
+ const bool root_added
+ )
{
- JointModelVariant root_joint = bp::extract<JointModelVariant> (root_joint_object);
- return boost::apply_visitor(BuildModelAndGeomVisitor(filename, package_dirs), root_joint);
+ GeometryModel * geometry_model = new GeometryModel(se3::urdf::buildGeom(*model, filename, root_added));
+
+ return GeometryModelHandler(geometry_model, true);
}
- static ModelGeometryHandlerPair_t
- buildModelAndGeomFromUrdf(const std::string & filename,
- const std::vector<std::string> & package_dirs)
+ static GeometryModelHandler
+ buildGeomFromUrdf(const ModelHandler & model,
+ const std::string & filename,
+ const std::vector<std::string> & package_dirs,
+ const bool root_added
+ )
{
- Model * model = new Model(se3::urdf::buildModel(filename));
- GeometryModel * geometry_model = new GeometryModel(se3::urdf::buildGeom(*model, filename, package_dirs));
+ GeometryModel * geometry_model = new GeometryModel(se3::urdf::buildGeom(*model, filename, package_dirs, root_added));
- return ModelGeometryHandlerPair_t (ModelHandler(model, true),
- GeometryModelHandler(geometry_model, true)
- );
+ return GeometryModelHandler(geometry_model, true);
}
#endif // #ifdef WITH_HPP_FCL
@@ -172,17 +151,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::vector<std::string> &, bp::object &)> (&ParsersPythonVisitor::buildModelAndGeomFromUrdf),
- bp::args("filename (string)", "package_dirs (vector of strings)",
- "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 &, const std::vector<std::string> &, const bool)> (&ParsersPythonVisitor::buildGeomFromUrdf),
+ bp::args("Model to assosiate the Geometry","filename (string)", "package_dirs (vector of strings)",
+ "bool stating if we added a custom root joint to the Model"),
+ "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::vector<std::string> &)> (&ParsersPythonVisitor::buildModelAndGeomFromUrdf),
- bp::args("filename (string)", "package_dirs (vector of strings)"),
- "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 &, const bool)> (&ParsersPythonVisitor::buildGeomFromUrdf),
+ bp::args("Model to assosiate the Geometry","filename (string)", "bool stating if we added a custom root joint to the Model"),
+ "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
diff --git a/src/python/robot_wrapper.py b/src/python/robot_wrapper.py
index 50efe8c7d3c7f84639cac49914a8a8ee2dffdcb6..faa3ad8d1211a72729e4872fbf4e9b83c63ae82d 100644
--- a/src/python/robot_wrapper.py
+++ b/src/python/robot_wrapper.py
@@ -22,32 +22,28 @@ 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:
+ self.root_added = False
+ else:
self.model = se3.buildModelFromUrdf(filename, root_joint)
-
- self.data = self.model.createData()
+ self.root_added = True
+
+ 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.root_added) # Will parse ROS_PACKAGE_PATH
+ 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.root_added)
+ self.geometry_data = se3.GeometryData(self.data, self.geometry_model)
self.v0 = utils.zero(self.nv)
self.q0 = utils.zero(self.nq)