9 #include <boost/python.hpp>
18 #ifdef PINOCCHIO_WITH_URDFDOM
19 typedef ::hpp::fcl::MeshLoaderPtr MeshLoaderPtr;
22 buildGeomFromUrdf_existing(
const Model &
model,
23 const std::istream & stream,
25 GeometryModel & geometry_model,
26 bp::object py_pkg_dirs,
27 bp::object py_mesh_loader)
29 MeshLoaderPtr mesh_loader = MeshLoaderPtr();
30 if (!py_mesh_loader.is_none()) {
31 #ifdef PINOCCHIO_WITH_HPP_FCL
34 mesh_loader = bp::extract<::hpp::fcl::MeshLoaderPtr>(py_mesh_loader);
37 PyErr_WarnEx(PyExc_UserWarning,
"Mesh loader is ignored because Pinocchio is not built with hpp-fcl", 1);
41 std::vector<std::string> pkg_dirs;
43 bp::extract<std::string> pkg_dir_extract(py_pkg_dirs);
44 bp::extract<bp::list> pkg_dirs_list_extract(py_pkg_dirs);
45 bp::extract<const std::vector<std::string>&> pkg_dirs_vect_extract(py_pkg_dirs);
46 if (py_pkg_dirs.is_none()) {}
47 else if (pkg_dir_extract.check())
48 pkg_dirs.push_back(pkg_dir_extract());
49 else if (pkg_dirs_list_extract.check())
50 extract(pkg_dirs_list_extract(), pkg_dirs);
51 else if (pkg_dirs_vect_extract.check())
52 pkg_dirs = pkg_dirs_vect_extract();
54 std::string what = bp::extract<std::string>(py_pkg_dirs.attr(
"__str__")())();
55 throw std::invalid_argument(
"pkg_dirs must be either None, a string or a list of strings. Provided " + what);
64 const std::istream & stream,
66 bp::object py_geom_model,
68 bp::object mesh_loader)
71 if (py_geom_model.is_none())
74 bp::extract<GeometryModel*> geom_model_extract(py_geom_model);
75 if (geom_model_extract.check())
80 PyErr_WarnEx(PyExc_UserWarning,
81 "You passed package dir(s) via argument geometry_model and provided package_dirs.",1);
85 bp::object new_pkg_dirs = py_geom_model;
87 throw std::invalid_argument(
"package_dirs and mesh_loader cannot be both provided since you passed the package dirs via argument geometry_model.");
88 if (mesh_loader.is_none())
95 }
catch (std::invalid_argument
const& e) {
96 std::cout <<
"Caught: " << e.what() << std::endl;
97 throw std::invalid_argument(
"Argument geometry_model should be a GeometryModel");
107 const std::string & filename,
111 bp::object mesh_loader)
113 std::ifstream stream(
filename.c_str());
114 if (!stream.is_open())
116 throw std::invalid_argument(filename +
" does not seem to be a valid file.");
123 const std::string & xmlString,
127 bp::object mesh_loader)
129 std::istringstream stream(xmlString);
133 #ifdef PINOCCHIO_WITH_HPP_FCL
134 # define MESH_LOADER_DOC "\tmesh_loader: an hpp-fcl mesh loader (to load only once the related geometries).\n"
135 #else // #ifdef PINOCCHIO_WITH_HPP_FCL
136 # define MESH_LOADER_DOC "\tmesh_loader: unused because the Pinocchio is built without hpp-fcl\n"
137 #endif // #ifdef PINOCCHIO_WITH_HPP_FCL
138 template <std::
size_t owner_arg = 1>
139 struct return_value_policy : bp::return_internal_reference<owner_arg>
142 template <
class ArgumentPackage>
143 static PyObject* postcall(ArgumentPackage
const& args_, PyObject* result)
147 PyObject* patient = bp::detail::get_prev<owner_arg>::execute(args_, result);
148 if (patient != Py_None)
149 return bp::return_internal_reference<owner_arg>::postcall(args_, result);
151 bp::extract<GeometryModel*> geom_model_extract(result);
152 if (geom_model_extract.check())
154 return bp::to_python_indirect<GeometryModel, bp::detail::make_owning_holder>()(geom_model_extract());
157 PyErr_SetString(PyExc_RuntimeError,
"pinocchio::python::return_value_policy only works on GeometryModel* data type");
164 void defBuildUrdf(
const char*
name, F f,
const char* urdf_arg,
const char* urdf_doc)
166 std::ostringstream doc;
167 doc <<
"Parse the URDF file given as input looking for the geometry of the given input model and\n"
168 "and store either the collision geometries (GeometryType.COLLISION) or the visual geometries (GeometryType.VISUAL) in a GeometryModel object.\n"
170 "\tmodel: model of the robot\n"
171 "\n" << urdf_arg <<
": " << urdf_doc <<
"\n"
172 "\tgeom_type: type of geometry to extract from the URDF file (either the VISUAL for display or the COLLISION for collision detection).\n"
173 "\tgeometry_model: if provided, this geometry model will be used to store the parsed information instead of creating a new one\n"
174 "\tpackage_dirs: either a single path or a vector of paths pointing to folders containing the model of the robot\n"
178 "\ta new GeometryModel if `geometry_model` is None else `geometry_model` (that has been updated).\n";
183 bp::arg(
"geom_type"),
184 bp::arg(
"geometry_model") =
static_cast<GeometryModel*
>(NULL),
185 bp::arg(
"package_dirs") = bp::object(),
186 bp::arg(
"mesh_loader") = bp::object()),
187 doc.str().c_str(), return_value_policy<4>());
194 #ifdef PINOCCHIO_WITH_URDFDOM
195 defBuildUrdf(
"buildGeomFromUrdf", buildGeomFromUrdfFile,
"urdf_filename",
196 "path to the URDF file containing the model of the robot");
197 defBuildUrdf(
"buildGeomFromUrdfString", buildGeomFromUrdfString,
"urdf_string",
198 "a string containing the URDF model of the robot");
199 #endif // #ifdef PINOCCHIO_WITH_URDFDOM