5 #ifdef PINOCCHIO_WITH_URDFDOM
11 #include <boost/python.hpp>
20 #ifdef PINOCCHIO_WITH_URDFDOM
21 typedef ::hpp::fcl::MeshLoaderPtr MeshLoaderPtr;
23 void buildGeomFromUrdf_existing(
25 const std::istream & stream,
27 GeometryModel & geometry_model,
28 bp::object py_pkg_dirs,
29 bp::object py_mesh_loader)
31 MeshLoaderPtr mesh_loader = MeshLoaderPtr();
32 if (!py_mesh_loader.is_none())
34 #ifdef PINOCCHIO_WITH_HPP_FCL
37 mesh_loader = bp::extract<::hpp::fcl::MeshLoaderPtr>(py_mesh_loader);
41 PyExc_UserWarning,
"Mesh loader is ignored because Pinocchio is not built with hpp-fcl",
46 std::vector<std::string> pkg_dirs;
48 bp::extract<std::string> pkg_dir_extract(py_pkg_dirs);
49 bp::extract<bp::list> pkg_dirs_list_extract(py_pkg_dirs);
50 bp::extract<const std::vector<std::string> &> pkg_dirs_vect_extract(py_pkg_dirs);
51 if (py_pkg_dirs.is_none())
54 else if (pkg_dir_extract.check())
55 pkg_dirs.push_back(pkg_dir_extract());
56 else if (pkg_dirs_list_extract.check())
57 extract(pkg_dirs_list_extract(), pkg_dirs);
58 else if (pkg_dirs_vect_extract.check())
59 pkg_dirs = pkg_dirs_vect_extract();
62 std::string what = bp::extract<std::string>(py_pkg_dirs.attr(
"__str__")())();
63 throw std::invalid_argument(
64 "pkg_dirs must be either None, a string or a list of strings. Provided " + what);
71 GeometryModel * buildGeomFromUrdfStream(
73 const std::istream & stream,
75 bp::object py_geom_model,
77 bp::object mesh_loader)
80 if (py_geom_model.is_none())
84 bp::extract<GeometryModel *> geom_model_extract(py_geom_model);
85 if (geom_model_extract.check())
93 "You passed package dir(s) via argument geometry_model and provided package_dirs.", 1);
97 bp::object new_pkg_dirs = py_geom_model;
99 throw std::invalid_argument(
100 "package_dirs and mesh_loader cannot be both provided since you passed the package "
101 "dirs via argument geometry_model.");
102 if (mesh_loader.is_none())
111 catch (std::invalid_argument
const & e)
113 std::cout <<
"Caught: " << e.what() << std::endl;
114 throw std::invalid_argument(
"Argument geometry_model should be a GeometryModel");
122 GeometryModel * buildGeomFromUrdfFile(
124 const std::string & filename,
128 bp::object mesh_loader)
130 std::ifstream stream(
filename.c_str());
131 if (!stream.is_open())
133 throw std::invalid_argument(filename +
" does not seem to be a valid file.");
138 GeometryModel * buildGeomFromUrdfString(
140 const std::string & xmlString,
144 bp::object mesh_loader)
146 std::istringstream stream(xmlString);
150 #ifdef PINOCCHIO_WITH_HPP_FCL
151 #define MESH_LOADER_DOC \
152 "\tmesh_loader: an hpp-fcl mesh loader (to load only once the related geometries).\n"
153 #else // #ifdef PINOCCHIO_WITH_HPP_FCL
154 #define MESH_LOADER_DOC "\tmesh_loader: unused because the Pinocchio is built without hpp-fcl\n"
155 #endif // #ifdef PINOCCHIO_WITH_HPP_FCL
156 template<std::
size_t owner_arg = 1>
157 struct return_value_policy : bp::return_internal_reference<owner_arg>
160 template<
class ArgumentPackage>
161 static PyObject * postcall(ArgumentPackage
const & args_, PyObject * result)
165 PyObject * patient = bp::detail::get_prev<owner_arg>::execute(args_, result);
166 if (patient != Py_None)
167 return bp::return_internal_reference<owner_arg>::postcall(args_, result);
169 bp::extract<GeometryModel *> geom_model_extract(result);
170 if (geom_model_extract.check())
172 return bp::to_python_indirect<GeometryModel, bp::detail::make_owning_holder>()(
173 geom_model_extract());
178 "pinocchio::python::return_value_policy only works on GeometryModel* data type");
184 void defBuildUrdf(
const char *
name, F
f,
const char * urdf_arg,
const char * urdf_doc)
186 std::ostringstream doc;
187 doc <<
"Parse the URDF file given as input looking for the geometry of the given input model "
189 "and store either the collision geometries (GeometryType.COLLISION) or the visual "
190 "geometries (GeometryType.VISUAL) in a GeometryModel object.\n"
192 "\tmodel: model of the robot\n"
194 << urdf_arg <<
": " << urdf_doc
196 "\tgeom_type: type of geometry to extract from the URDF file (either the VISUAL for "
197 "display or the COLLISION for collision detection).\n"
198 "\tgeometry_model: if provided, this geometry model will be used to store the parsed "
199 "information instead of creating a new one\n"
200 "\tpackage_dirs: either a single path or a vector of paths pointing to folders "
201 "containing the model of the robot\n" MESH_LOADER_DOC
"\n"
203 "\ta new GeometryModel if `geometry_model` is None else `geometry_model` (that has "
208 (bp::arg(
"model"), bp::arg(urdf_arg), bp::arg(
"geom_type"),
209 bp::arg(
"geometry_model") =
static_cast<GeometryModel *
>(NULL),
210 bp::arg(
"package_dirs") = bp::object(), bp::arg(
"mesh_loader") = bp::object()),
211 doc.str().c_str(), return_value_policy<4>());
218 #ifdef PINOCCHIO_WITH_URDFDOM
220 "buildGeomFromUrdf", buildGeomFromUrdfFile,
"urdf_filename",
221 "path to the URDF file containing the model of the robot");
223 "buildGeomFromUrdfString", buildGeomFromUrdfString,
"urdf_string",
224 "a string containing the URDF model of the robot");
225 #endif // #ifdef PINOCCHIO_WITH_URDFDOM