Commit bec54309 authored by Valenza Florian's avatar Valenza Florian
Browse files

[Python] Update methods and RobotWrapper to use buildGeom instead of...

[Python] Update methods and RobotWrapper to use buildGeom instead of buildModelAndGeom and to use a list of package dirs instead of one meshRootDir.If no package_dirs is given by the user, then it will call the C++ procedure that automatically parse the ROS_PACKAGE_PATH
parent 5f2cb66f
......@@ -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
......
......@@ -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)
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment