Unverified Commit ef9a221b authored by Justin Carpentier's avatar Justin Carpentier Committed by GitHub
Browse files

Merge pull request #1538 from jmirabel/python_geom_from_urdf_string

[python] Simplify buildGeomFromUrdf and allow to build from string.
parents 6ba84c4a dec993cd
......@@ -4,6 +4,7 @@
#include "pinocchio/parsers/urdf.hpp"
#include "pinocchio/bindings/python/parsers/urdf.hpp"
#include "pinocchio/bindings/python/utils/list.hpp"
#include <boost/python.hpp>
......@@ -15,332 +16,173 @@ namespace pinocchio
namespace bp = boost::python;
#ifdef PINOCCHIO_WITH_URDFDOM
GeometryModel
buildGeomFromUrdf(const Model & model,
const std::string & filename,
const GeometryType type)
typedef ::hpp::fcl::MeshLoaderPtr MeshLoaderPtr;
void
buildGeomFromUrdf_existing(const Model & model,
const std::istream & stream,
const GeometryType type,
GeometryModel & geometry_model,
bp::object py_pkg_dirs,
bp::object py_mesh_loader)
{
GeometryModel geometry_model;
pinocchio::urdf::buildGeom(model,filename,type,geometry_model);
return geometry_model;
MeshLoaderPtr mesh_loader = MeshLoaderPtr();
if (!py_mesh_loader.is_none()) {
#ifdef PINOCCHIO_WITH_HPP_FCL
mesh_loader = bp::extract<::hpp::fcl::MeshLoaderPtr>(py_mesh_loader);
#else
PyErr_WarnEx(PyExc_UserWarning, "Mesh loader is ignored because Pinocchio is not built with hpp-fcl", 1);
#endif
}
std::vector<std::string> pkg_dirs;
bp::extract<std::string> pkg_dir_extract(py_pkg_dirs);
bp::extract<bp::list> pkg_dirs_list_extract(py_pkg_dirs);
bp::extract<const std::vector<std::string>&> pkg_dirs_vect_extract(py_pkg_dirs);
if (py_pkg_dirs.is_none()) {} // Provided None
else if (pkg_dir_extract.check()) // Provided a string
pkg_dirs.push_back(pkg_dir_extract());
else if (pkg_dirs_list_extract.check()) // Provided a list of string
extract(pkg_dirs_list_extract(), pkg_dirs);
else if (pkg_dirs_vect_extract.check()) // Provided a vector of string
pkg_dirs = pkg_dirs_vect_extract();
else { // Did not understand the provided argument
std::string what = bp::extract<std::string>(py_pkg_dirs.attr("__str__")())();
throw std::invalid_argument("pkg_dirs must be either None, a string or a list of strings. Provided " + what);
}
pinocchio::urdf::buildGeom(model,stream,type,geometry_model,pkg_dirs,mesh_loader);
}
GeometryModel &
buildGeomFromUrdf(const Model & model,
const std::string & filename,
const GeometryType type,
GeometryModel & geometry_model)
// This function is complex in order to keep backward compatibility.
GeometryModel*
buildGeomFromUrdfStream(const Model & model,
const std::istream & stream,
const GeometryType type,
bp::object py_geom_model,
bp::object package_dirs,
bp::object mesh_loader)
{
pinocchio::urdf::buildGeom(model,filename,type,geometry_model);
return geometry_model;
GeometryModel* geom_model;
if (py_geom_model.is_none())
geom_model = new GeometryModel;
else {
bp::extract<GeometryModel*> geom_model_extract(py_geom_model);
if (geom_model_extract.check())
geom_model = geom_model_extract();
else {
// When backward compat is removed, the code in this `else` section
// can be removed and the argument py_geom_model changed into a GeometryModel*
PyErr_WarnEx(PyExc_UserWarning,
"You passed package dir(s) via argument geometry_model and provided package_dirs.",1);
// At this stage, py_geom_model contains the package dir(s). mesh_loader can
// be passed either by package_dirs or mesh_loader
bp::object new_pkg_dirs = py_geom_model;
if (!package_dirs.is_none() && !mesh_loader.is_none())
throw std::invalid_argument("package_dirs and mesh_loader cannot be both provided since you passed the package dirs via argument geometry_model.");
if (mesh_loader.is_none())
mesh_loader = package_dirs;
try {
// If geom_model is not a valid package_dir(s), then rethrow with clearer message
geom_model = new GeometryModel;
buildGeomFromUrdf_existing(model, stream, type, *geom_model, new_pkg_dirs, mesh_loader);
return geom_model;
} catch (std::invalid_argument const& e) {
std::cout << "Caught: " << e.what() << std::endl;
throw std::invalid_argument("Argument geometry_model should be a GeometryModel");
}
}
}
buildGeomFromUrdf_existing(model, stream, type, *geom_model, package_dirs, mesh_loader);
return geom_model;
}
GeometryModel
buildGeomFromUrdf(const Model & model,
const std::string & filename,
const GeometryType type,
const std::vector<std::string> & package_dirs)
GeometryModel*
buildGeomFromUrdfFile(const Model & model,
const std::string & filename,
const GeometryType type,
bp::object geom_model,
bp::object package_dirs,
bp::object mesh_loader)
{
GeometryModel geometry_model;
pinocchio::urdf::buildGeom(model,filename,type,geometry_model,package_dirs);
return geometry_model;
std::ifstream stream(filename.c_str());
if (!stream.is_open())
{
throw std::invalid_argument(filename + " does not seem to be a valid file.");
}
return buildGeomFromUrdfStream(model, stream, type, geom_model, package_dirs, mesh_loader);
}
GeometryModel &
buildGeomFromUrdf(const Model & model,
const std::string & filename,
const GeometryType type,
GeometryModel & geometry_model,
const std::vector<std::string> & package_dirs)
GeometryModel*
buildGeomFromUrdfString(const Model & model,
const std::string & xmlString,
const GeometryType type,
bp::object geom_model,
bp::object package_dirs,
bp::object mesh_loader)
{
pinocchio::urdf::buildGeom(model,filename,type,geometry_model,package_dirs);
return geometry_model;
std::istringstream stream(xmlString);
return buildGeomFromUrdfStream(model, stream, type, geom_model, package_dirs, mesh_loader);
}
GeometryModel
buildGeomFromUrdf(const Model & model,
const std::string & filename,
const GeometryType type,
const std::string & package_dir)
{
GeometryModel geometry_model;
pinocchio::urdf::buildGeom(model,filename,type,geometry_model,package_dir);
return geometry_model;
}
GeometryModel &
buildGeomFromUrdf(const Model & model,
const std::string & filename,
const GeometryType type,
GeometryModel & geometry_model,
const std::string & package_dir)
{
pinocchio::urdf::buildGeom(model,filename,type,geometry_model,package_dir);
return geometry_model;
}
#ifdef PINOCCHIO_WITH_HPP_FCL
GeometryModel
buildGeomFromUrdf(const Model & model,
const std::string & filename,
const GeometryType type,
const fcl::MeshLoaderPtr & meshLoader)
{
std::vector<std::string> hints;
GeometryModel geometry_model;
pinocchio::urdf::buildGeom(model,filename,type,geometry_model,hints,meshLoader);
return geometry_model;
}
GeometryModel &
buildGeomFromUrdf(const Model & model,
const std::string & filename,
const GeometryType type,
GeometryModel & geometry_model,
const fcl::MeshLoaderPtr & meshLoader)
{
std::vector<std::string> hints;
pinocchio::urdf::buildGeom(model,filename,type,geometry_model,hints,meshLoader);
return geometry_model;
}
GeometryModel
buildGeomFromUrdf(const Model & model,
const std::string & filename,
const GeometryType type,
const std::vector<std::string> & package_dirs,
const fcl::MeshLoaderPtr & meshLoader)
{
GeometryModel geometry_model;
pinocchio::urdf::buildGeom(model,filename,type,geometry_model,package_dirs,meshLoader);
return geometry_model;
}
GeometryModel &
buildGeomFromUrdf(const Model & model,
const std::string & filename,
const GeometryType type,
GeometryModel & geometry_model,
const std::vector<std::string> & package_dirs,
const fcl::MeshLoaderPtr & meshLoader)
# define MESH_LOADER_DOC "\tmesh_loader: an hpp-fcl mesh loader (to load only once the related geometries).\n"
#else // #ifdef PINOCCHIO_WITH_HPP_FCL
# define MESH_LOADER_DOC "\tmesh_loader: unused because the Pinocchio is built without hpp-fcl\n"
#endif // #ifdef PINOCCHIO_WITH_HPP_FCL
template <std::size_t owner_arg = 1>
struct return_value_policy : bp::return_internal_reference<owner_arg>
{
pinocchio::urdf::buildGeom(model,filename,type,geometry_model,package_dirs,meshLoader);
return geometry_model;
}
GeometryModel
buildGeomFromUrdf(const Model & model,
const std::string & filename,
const GeometryType type,
const std::string & package_dir,
const fcl::MeshLoaderPtr & meshLoader)
public:
template <class ArgumentPackage>
static PyObject* postcall(ArgumentPackage const& args_, PyObject* result)
{
PyObject* patient = bp::detail::get_prev<owner_arg>::execute(args_, result);
if (patient != Py_None)
return bp::return_internal_reference<owner_arg>::postcall(args_, result);
return result;
}
};
template<typename F>
void defBuildUrdf(const char* name, F f, const char* urdf_arg, const char* urdf_doc)
{
GeometryModel geometry_model;
pinocchio::urdf::buildGeom(model,filename,type,geometry_model,package_dir,meshLoader);
return geometry_model;
std::ostringstream doc;
doc << "Parse the URDF file given as input looking for the geometry of the given input model and\n"
"and store either the collision geometries (GeometryType.COLLISION) or the visual geometries (GeometryType.VISUAL) in a GeometryModel object.\n"
"Parameters:\n"
"\tmodel: model of the robot\n"
"\n" << urdf_arg << ": " << urdf_doc << "\n"
"\tgeom_type: type of geometry to extract from the URDF file (either the VISUAL for display or the COLLISION for collision detection).\n"
"\tgeometry_model: if provided, this geometry model will be used to store the parsed information instead of creating a new one\n"
"\tpackage_dirs: either a single path or a vector of paths pointing to folders containing the model of the robot\n"
MESH_LOADER_DOC
"\n"
"Retuns:\n"
"\ta new GeometryModel if `geometry_model` is None else `geometry_model` (that has been updated).\n";
bp::def(name, f,
(bp::arg("model"),
bp::arg(urdf_arg),
bp::arg("geom_type"),
bp::arg("geometry_model") = static_cast<GeometryModel*>(NULL),
bp::arg("package_dirs") = bp::object(),
bp::arg("mesh_loader") = bp::object()),
doc.str().c_str(), return_value_policy<4>());
}
GeometryModel &
buildGeomFromUrdf(const Model & model,
const std::string & filename,
const GeometryType type,
GeometryModel & geometry_model,
const std::string & package_dir,
const fcl::MeshLoaderPtr & meshLoader)
{
pinocchio::urdf::buildGeom(model,filename,type,geometry_model,package_dir,meshLoader);
return geometry_model;
}
#endif // #ifdef PINOCCHIO_WITH_HPP_FCL
#endif
void exposeURDFGeometry()
{
#ifdef PINOCCHIO_WITH_URDFDOM
bp::def("buildGeomFromUrdf",
static_cast <GeometryModel (*) (const Model &, const std::string &, const GeometryType, const std::vector<std::string> &)> (pinocchio::python::buildGeomFromUrdf),
bp::args("model","urdf_filename","geom_type","package_dirs"),
"Parse the URDF file given as input looking for the geometry of the given input model and\n"
"return a GeometryModel containing either the collision geometries (GeometryType.COLLISION) or the visual geometries (GeometryType.VISUAL).\n"
"Parameters:\n"
"\tmodel: model of the robot\n"
"\turdf_filename: path to the URDF file containing the model of the robot\n"
"\tgeom_type: type of geometry to extract from the URDF file (either the VISUAL for display or the COLLISION for collision detection).\n"
"\tpackage_dirs: vector of paths pointing to the folders containing the model of the robot\n"
);
bp::def("buildGeomFromUrdf",
static_cast <GeometryModel & (*) (const Model &, const std::string &, const GeometryType, GeometryModel &, const std::vector<std::string> &)> (pinocchio::python::buildGeomFromUrdf),
bp::args("model","urdf_filename","geom_type","geom_model","package_dirs"),
"Parse the URDF file given as input looking for the geometry of the given input model and\n"
"and store either the collision geometries (GeometryType.COLLISION) or the visual geometries (GeometryType.VISUAL) in the geom_model given as input.\n"
"Parameters:\n"
"\tmodel: model of the robot\n"
"\turdf_filename: path to the URDF file containing the model of the robot\n"
"\tgeom_type: type of geometry to extract from the URDF file (either the VISUAL for display or the COLLISION for collision detection).\n"
"\tgeom_model: reference where to store the parsed information\n"
"\tpackage_dirs: vector of paths pointing to the folders containing the model of the robot\n",
bp::return_internal_reference<4>()
);
bp::def("buildGeomFromUrdf",
static_cast <GeometryModel (*) (const Model &, const std::string &, const GeometryType)> (pinocchio::python::buildGeomFromUrdf),
bp::args("model","urdf_filename","geom_type"),
"Parse the URDF file given as input looking for the geometry of the given input model and\n"
"return a GeometryModel containing either the collision geometries (GeometryType.COLLISION) or the visual geometries (GeometryType.VISUAL).\n"
"Parameters:\n"
"\tmodel: model of the robot\n"
"\turdf_filename: path to the URDF file containing the model of the robot\n"
"\tgeom_type: type of geometry to extract from the URDF file (either the VISUAL for display or the COLLISION for collision detection).\n"
"Note:\n"
"This function does not take any hint concerning the location of the meshes of the robot."
);
bp::def("buildGeomFromUrdf",
static_cast <GeometryModel & (*) (const Model &, const std::string &, const GeometryType, GeometryModel &)> (pinocchio::python::buildGeomFromUrdf),
bp::args("model","urdf_filename","geom_type","geom_model"),
"Parse the URDF file given as input looking for the geometry of the given input model and\n"
"and store either the collision geometries (GeometryType.COLLISION) or the visual geometries (GeometryType.VISUAL) in the geom_model given as input.\n"
"Parameters:\n"
"\tmodel: model of the robot\n"
"\turdf_filename: path to the URDF file containing the model of the robot\n"
"\tgeom_type: type of geometry to extract from the URDF file (either the VISUAL for display or the COLLISION for collision detection).\n"
"\tgeom_model: reference where to store the parsed information\n"
"Note:\n"
"This function does not take any hint concerning the location of the meshes of the robot.",
bp::return_internal_reference<4>()
);
bp::def("buildGeomFromUrdf",
static_cast <GeometryModel (*) (const Model &, const std::string &, const GeometryType, const std::string &)> (pinocchio::python::buildGeomFromUrdf),
bp::args("model","urdf_filename","geom_type","package_dir" ),
"Parse the URDF file given as input looking for the geometry of the given input model and\n"
"return a GeometryModel containing either the collision geometries (GeometryType.COLLISION) or the visual geometries (GeometryType.VISUAL).\n"
"Parameters:\n"
"\tmodel: model of the robot\n"
"\turdf_filename: path to the URDF file containing the model of the robot\n"
"\tgeom_type: type of geometry to extract from the URDF file (either the VISUAL for display or the COLLISION for collision detection).\n"
"\tpackage_dir: path pointing to the folder containing the meshes of the robot\n"
);
bp::def("buildGeomFromUrdf",
static_cast <GeometryModel & (*) (const Model &, const std::string &, const GeometryType, GeometryModel &, const std::string &)> (pinocchio::python::buildGeomFromUrdf),
bp::args("model","urdf_filename","geom_type","geom_model","package_dir"),
"Parse the URDF file given as input looking for the geometry of the given input model and\n"
"and store either the collision geometries (GeometryType.COLLISION) or the visual geometries (GeometryType.VISUAL) in the geom_model given as input.\n"
"Parameters:\n"
"\tmodel: model of the robot\n"
"\turdf_filename: path to the URDF file containing the model of the robot\n"
"\tgeom_type: type of geometry to extract from the URDF file (either the VISUAL for display or the COLLISION for collision detection).\n"
"\tgeom_model: reference where to store the parsed information\n"
"\tpackage_dir: path pointing to the folder containing the meshes of the robot\n",
bp::return_internal_reference<4>()
);
#ifdef PINOCCHIO_WITH_HPP_FCL
bp::def("buildGeomFromUrdf",
static_cast <GeometryModel (*) (const Model &, const std::string &, const GeometryType, const std::vector<std::string> &, const fcl::MeshLoaderPtr&)> (pinocchio::python::buildGeomFromUrdf),
bp::args("model","urdf_filename","geom_type","package_dirs","mesh_loader"),
"Parse the URDF file given as input looking for the geometry of the given input model and\n"
"return a GeometryModel containing either the collision geometries (GeometryType.COLLISION) or the visual geometries (GeometryType.VISUAL).\n"
"Parameters:\n"
"\tmodel: model of the robot\n"
"\turdf_filename: path to the URDF file containing the model of the robot\n"
"\tgeom_type: type of geometry to extract from the URDF file (either the VISUAL for display or the COLLISION for collision detection).\n"
"\tpackage_dirs: vector of paths pointing to the folders containing the model of the robot\n"
"\tmesh_loader: an hpp-fcl mesh loader (to load only once the related geometries)."
);
bp::def("buildGeomFromUrdf",
static_cast <GeometryModel & (*) (const Model &, const std::string &, const GeometryType, GeometryModel &, const std::vector<std::string> &, const fcl::MeshLoaderPtr&)> (pinocchio::python::buildGeomFromUrdf),
bp::args("model","urdf_filename","geom_type","geom_model","package_dirs","mesh_loader"),
"Parse the URDF file given as input looking for the geometry of the given input model and\n"
"and store either the collision geometries (GeometryType.COLLISION) or the visual geometries (GeometryType.VISUAL) in the geom_model given as input.\n"
"Parameters:\n"
"\tmodel: model of the robot\n"
"\turdf_filename: path to the URDF file containing the model of the robot\n"
"\tgeom_type: type of geometry to extract from the URDF file (either the VISUAL for display or the COLLISION for collision detection).\n"
"\tgeom_model: reference where to store the parsed information\n"
"\tpackage_dirs: vector of paths pointing to the folders containing the model of the robot\n"
"\tmesh_loader: an hpp-fcl mesh loader (to load only once the related geometries).",
bp::return_internal_reference<4>()
);
bp::def("buildGeomFromUrdf",
static_cast <GeometryModel (*) (const Model &, const std::string &, const GeometryType, const std::string &, const fcl::MeshLoaderPtr&)> (pinocchio::python::buildGeomFromUrdf),
bp::args("model","urdf_filename","geom_type","package_dir","mesh_loader"),
"Parse the URDF file given as input looking for the geometry of the given input model and\n"
"return a GeometryModel containing either the collision geometries (GeometryType.COLLISION) or the visual geometries (GeometryType.VISUAL).\n"
"Parameters:\n"
"\tmodel: model of the robot\n"
"\turdf_filename: path to the URDF file containing the model of the robot\n"
"\tgeom_type: type of geometry to extract from the URDF file (either the VISUAL for display or the COLLISION for collision detection).\n"
"\tpackage_dir: path pointing to the folder containing the meshes of the robot\n"
"\tmesh_loader: an hpp-fcl mesh loader (to load only once the related geometries)."
);
bp::def("buildGeomFromUrdf",
static_cast <GeometryModel & (*) (const Model &, const std::string &, const GeometryType, GeometryModel &, const std::string &, const fcl::MeshLoaderPtr&)> (pinocchio::python::buildGeomFromUrdf),
bp::args("model","urdf_filename","geom_type","geom_model","package_dir","mesh_loader"),
"Parse the URDF file given as input looking for the geometry of the given input model and\n"
"and store either the collision geometries (GeometryType.COLLISION) or the visual geometries (GeometryType.VISUAL) in the geom_model given as input.\n"
"Parameters:\n"
"\tmodel: model of the robot\n"
"\turdf_filename: path to the URDF file containing the model of the robot\n"
"\tgeom_type: type of geometry to extract from the URDF file (either the VISUAL for display or the COLLISION for collision detection).\n"
"\tgeom_model: reference where to store the parsed information\n"
"\tpackage_dir: path pointing to the folder containing the meshes of the robot\n"
"\tmesh_loader: an hpp-fcl mesh loader (to load only once the related geometries).",
bp::return_internal_reference<4>()
);
bp::def("buildGeomFromUrdf",
static_cast <GeometryModel (*) (const Model &, const std::string &, const GeometryType, const fcl::MeshLoaderPtr&)> (pinocchio::python::buildGeomFromUrdf),
bp::args("model","urdf_filename","geom_type","mesh_loader"),
"Parse the URDF file given as input looking for the geometry of the given input model and\n"
"return a GeometryModel containing either the collision geometries (GeometryType.COLLISION) or the visual geometries (GeometryType.VISUAL).\n"
"Parameters:\n"
"\tmodel: model of the robot\n"
"\turdf_filename: path to the URDF file containing the model of the robot\n"
"\tgeom_type: type of geometry to extract from the URDF file (either the VISUAL for display or the COLLISION for collision detection).\n"
"\tmesh_loader: an hpp-fcl mesh loader (to load only once the related geometries).\n"
"Note:\n"
"This function does not take any hint concerning the location of the meshes of the robot."
);
bp::def("buildGeomFromUrdf",
static_cast <GeometryModel & (*) (const Model &, const std::string &, const GeometryType, GeometryModel &, const fcl::MeshLoaderPtr&)> (pinocchio::python::buildGeomFromUrdf),
bp::args("model","urdf_filename","geom_type","geom_model","mesh_loader"),
"Parse the URDF file given as input looking for the geometry of the given input model and\n"
"and store either the collision geometries (GeometryType.COLLISION) or the visual geometries (GeometryType.VISUAL) in the geom_model given as input.\n"
"Parameters:\n"
"\tmodel: model of the robot\n"
"\turdf_filename: path to the URDF file containing the model of the robot\n"
"\tgeom_type: type of geometry to extract from the URDF file (either the VISUAL for display or the COLLISION for collision detection).\n"
"\tgeom_model: reference where to store the parsed information\n"
"\tmesh_loader: an hpp-fcl mesh loader (to load only once the related geometries).\n"
"Note:\n"
"This function does not take any hint concerning the location of the meshes of the robot.",
bp::return_internal_reference<4>()
);
#endif // #ifdef PINOCCHIO_WITH_HPP_FCL
defBuildUrdf("buildGeomFromUrdf", buildGeomFromUrdfFile, "urdf_filename",
"path to the URDF file containing the model of the robot");
defBuildUrdf("buildGeomFromUrdfString", buildGeomFromUrdfString, "urdf_string",
"a string containing the URDF model of the robot");
#endif // #ifdef PINOCCHIO_WITH_URDFDOM
}
}
......
......@@ -104,7 +104,7 @@ def se3ToXYZQUAT(M):
def XYZQUATToSe3(x):
return pin.XYZQUATToSE3(x)
def buildGeomFromUrdf(model, filename, *args):
def buildGeomFromUrdf(model, filename, *args, **kwargs):
arg3 = args[0]
if isinstance(arg3,(str,list,pin.StdVec_StdString)):
......@@ -116,14 +116,14 @@ def buildGeomFromUrdf(model, filename, *args):
message = ("This function signature is now deprecated and will be removed in future releases of Pinocchio. "
"Please change for the new signature buildGeomFromUrdf(model,filename,type,package_dirs,mesh_loader).")
_warnings.warn(message, category=DeprecatedWarning, stacklevel=2)
return pin.buildGeomFromUrdf(model,filename,geom_type,package_dir,mesh_loader)
return pin.buildGeomFromUrdf(model,filename,geom_type,package_dir,mesh_loader, **kwargs)
else:
message = ("This function signature is now deprecated and will be removed in future releases of Pinocchio. "
"Please change for the new signature buildGeomFromUrdf(model,filename,type,package_dirs).")
_warnings.warn(message, category=DeprecatedWarning, stacklevel=2)
return pin.buildGeomFromUrdf(model,filename,geom_type,package_dir)
return pin.buildGeomFromUrdf(model,filename,geom_type,package_dir, **kwargs)
else:
return pin.buildGeomFromUrdf(model, filename, *args)
return pin.buildGeomFromUrdf(model, filename, *args, **kwargs)
buildGeomFromUrdf.__doc__ = (
pin.buildGeomFromUrdf.__doc__
......
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