| Directory: | ./ |
|---|---|
| File: | bindings/python/parsers/urdf/geometry.cpp |
| Date: | 2024-08-27 18:20:05 |
| Exec | Total | Coverage | |
|---|---|---|---|
| Lines: | 53 | 74 | 71.6% |
| Branches: | 65 | 190 | 34.2% |
| Line | Branch | Exec | Source |
|---|---|---|---|
| 1 | // | ||
| 2 | // Copyright (c) 2015-2022 CNRS INRIA | ||
| 3 | // | ||
| 4 | |||
| 5 | #ifdef PINOCCHIO_WITH_URDFDOM | ||
| 6 | #include "pinocchio/parsers/urdf.hpp" | ||
| 7 | #endif | ||
| 8 | #include "pinocchio/bindings/python/parsers/urdf.hpp" | ||
| 9 | #include "pinocchio/bindings/python/utils/list.hpp" | ||
| 10 | |||
| 11 | #include <boost/python.hpp> | ||
| 12 | |||
| 13 | namespace pinocchio | ||
| 14 | { | ||
| 15 | namespace python | ||
| 16 | { | ||
| 17 | |||
| 18 | namespace bp = boost::python; | ||
| 19 | |||
| 20 | #ifdef PINOCCHIO_WITH_URDFDOM | ||
| 21 | typedef ::hpp::fcl::MeshLoaderPtr MeshLoaderPtr; | ||
| 22 | |||
| 23 | 23 | void buildGeomFromUrdf_existing( | |
| 24 | const Model & model, | ||
| 25 | const std::istream & stream, | ||
| 26 | const GeometryType type, | ||
| 27 | GeometryModel & geometry_model, | ||
| 28 | bp::object py_pkg_dirs, | ||
| 29 | bp::object py_mesh_loader) | ||
| 30 | { | ||
| 31 | 23 | MeshLoaderPtr mesh_loader = MeshLoaderPtr(); | |
| 32 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 23 times.
|
23 | if (!py_mesh_loader.is_none()) |
| 33 | { | ||
| 34 | #ifdef PINOCCHIO_WITH_HPP_FCL | ||
| 35 | PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH | ||
| 36 | PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED | ||
| 37 | ✗ | mesh_loader = bp::extract<::hpp::fcl::MeshLoaderPtr>(py_mesh_loader); | |
| 38 | PINOCCHIO_COMPILER_DIAGNOSTIC_POP | ||
| 39 | #else | ||
| 40 | PyErr_WarnEx( | ||
| 41 | PyExc_UserWarning, "Mesh loader is ignored because Pinocchio is not built with hpp-fcl", | ||
| 42 | 1); | ||
| 43 | #endif | ||
| 44 | } | ||
| 45 | |||
| 46 | 23 | std::vector<std::string> pkg_dirs; | |
| 47 | |||
| 48 |
1/2✓ Branch 1 taken 23 times.
✗ Branch 2 not taken.
|
23 | bp::extract<std::string> pkg_dir_extract(py_pkg_dirs); |
| 49 |
1/2✓ Branch 1 taken 23 times.
✗ Branch 2 not taken.
|
23 | bp::extract<bp::list> pkg_dirs_list_extract(py_pkg_dirs); |
| 50 |
1/2✓ Branch 1 taken 23 times.
✗ Branch 2 not taken.
|
23 | bp::extract<const std::vector<std::string> &> pkg_dirs_vect_extract(py_pkg_dirs); |
| 51 |
1/2✓ Branch 1 taken 23 times.
✗ Branch 2 not taken.
|
23 | if (py_pkg_dirs.is_none()) |
| 52 | { | ||
| 53 | } // Provided None | ||
| 54 |
1/2✓ Branch 1 taken 23 times.
✗ Branch 2 not taken.
|
23 | else if (pkg_dir_extract.check()) // Provided a string |
| 55 |
2/4✓ Branch 1 taken 23 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 23 times.
✗ Branch 5 not taken.
|
23 | pkg_dirs.push_back(pkg_dir_extract()); |
| 56 | ✗ | else if (pkg_dirs_list_extract.check()) // Provided a list of string | |
| 57 | ✗ | extract(pkg_dirs_list_extract(), pkg_dirs); | |
| 58 | ✗ | else if (pkg_dirs_vect_extract.check()) // Provided a vector of string | |
| 59 | ✗ | pkg_dirs = pkg_dirs_vect_extract(); | |
| 60 | else | ||
| 61 | { // Did not understand the provided argument | ||
| 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); | |
| 65 | } | ||
| 66 | |||
| 67 |
1/2✓ Branch 2 taken 23 times.
✗ Branch 3 not taken.
|
23 | pinocchio::urdf::buildGeom(model, stream, type, geometry_model, pkg_dirs, mesh_loader); |
| 68 | 23 | } | |
| 69 | |||
| 70 | // This function is complex in order to keep backward compatibility. | ||
| 71 | 23 | GeometryModel * buildGeomFromUrdfStream( | |
| 72 | const Model & model, | ||
| 73 | const std::istream & stream, | ||
| 74 | const GeometryType type, | ||
| 75 | bp::object py_geom_model, | ||
| 76 | bp::object package_dirs, | ||
| 77 | bp::object mesh_loader) | ||
| 78 | { | ||
| 79 | GeometryModel * geom_model; | ||
| 80 |
2/2✓ Branch 1 taken 22 times.
✓ Branch 2 taken 1 times.
|
23 | if (py_geom_model.is_none()) |
| 81 |
1/2✓ Branch 2 taken 22 times.
✗ Branch 3 not taken.
|
22 | geom_model = new GeometryModel; |
| 82 | else | ||
| 83 | { | ||
| 84 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | bp::extract<GeometryModel *> geom_model_extract(py_geom_model); |
| 85 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 1 times.
|
1 | if (geom_model_extract.check()) |
| 86 | ✗ | geom_model = geom_model_extract(); | |
| 87 | else | ||
| 88 | { | ||
| 89 | // When backward compat is removed, the code in this `else` section | ||
| 90 | // can be removed and the argument py_geom_model changed into a GeometryModel* | ||
| 91 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | PyErr_WarnEx( |
| 92 | PyExc_UserWarning, | ||
| 93 | "You passed package dir(s) via argument geometry_model and provided package_dirs.", 1); | ||
| 94 | |||
| 95 | // At this stage, py_geom_model contains the package dir(s). mesh_loader can | ||
| 96 | // be passed either by package_dirs or mesh_loader | ||
| 97 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | bp::object new_pkg_dirs = py_geom_model; |
| 98 |
2/6✗ Branch 1 not taken.
✓ Branch 2 taken 1 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✓ Branch 7 taken 1 times.
|
1 | if (!package_dirs.is_none() && !mesh_loader.is_none()) |
| 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 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | if (mesh_loader.is_none()) |
| 103 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | mesh_loader = package_dirs; |
| 104 | try | ||
| 105 | { | ||
| 106 | // If geom_model is not a valid package_dir(s), then rethrow with clearer message | ||
| 107 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
1 | geom_model = new GeometryModel; |
| 108 |
3/6✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
|
1 | buildGeomFromUrdf_existing(model, stream, type, *geom_model, new_pkg_dirs, mesh_loader); |
| 109 | 1 | return geom_model; | |
| 110 | } | ||
| 111 | ✗ | catch (std::invalid_argument const & e) | |
| 112 | { | ||
| 113 | ✗ | std::cout << "Caught: " << e.what() << std::endl; | |
| 114 | ✗ | throw std::invalid_argument("Argument geometry_model should be a GeometryModel"); | |
| 115 | } | ||
| 116 | 1 | } | |
| 117 | } | ||
| 118 |
2/4✓ Branch 2 taken 22 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 22 times.
✗ Branch 6 not taken.
|
22 | buildGeomFromUrdf_existing(model, stream, type, *geom_model, package_dirs, mesh_loader); |
| 119 | 22 | return geom_model; | |
| 120 | } | ||
| 121 | |||
| 122 | 23 | GeometryModel * buildGeomFromUrdfFile( | |
| 123 | const Model & model, | ||
| 124 | const std::string & filename, | ||
| 125 | const GeometryType type, | ||
| 126 | bp::object geom_model, | ||
| 127 | bp::object package_dirs, | ||
| 128 | bp::object mesh_loader) | ||
| 129 | { | ||
| 130 |
1/2✓ Branch 2 taken 23 times.
✗ Branch 3 not taken.
|
23 | std::ifstream stream(filename.c_str()); |
| 131 |
2/4✓ Branch 1 taken 23 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 23 times.
|
23 | if (!stream.is_open()) |
| 132 | { | ||
| 133 | ✗ | throw std::invalid_argument(filename + " does not seem to be a valid file."); | |
| 134 | } | ||
| 135 |
4/8✓ Branch 1 taken 23 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 23 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 23 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 23 times.
✗ Branch 11 not taken.
|
46 | return buildGeomFromUrdfStream(model, stream, type, geom_model, package_dirs, mesh_loader); |
| 136 | 23 | } | |
| 137 | |||
| 138 | ✗ | GeometryModel * buildGeomFromUrdfString( | |
| 139 | const Model & model, | ||
| 140 | const std::string & xmlString, | ||
| 141 | const GeometryType type, | ||
| 142 | bp::object geom_model, | ||
| 143 | bp::object package_dirs, | ||
| 144 | bp::object mesh_loader) | ||
| 145 | { | ||
| 146 | ✗ | std::istringstream stream(xmlString); | |
| 147 | ✗ | return buildGeomFromUrdfStream(model, stream, type, geom_model, package_dirs, mesh_loader); | |
| 148 | } | ||
| 149 | |||
| 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> | ||
| 158 | { | ||
| 159 | public: | ||
| 160 | template<class ArgumentPackage> | ||
| 161 | 23 | static PyObject * postcall(ArgumentPackage const & args_, PyObject * result) | |
| 162 | { | ||
| 163 | // If owner_arg exists, we run bp::return_internal_reference postcall | ||
| 164 | // result lifetime will be tied to the owner_arg lifetime | ||
| 165 | 23 | PyObject * patient = bp::detail::get_prev<owner_arg>::execute(args_, result); | |
| 166 |
2/2✓ Branch 0 taken 1 times.
✓ Branch 1 taken 22 times.
|
23 | if (patient != Py_None) |
| 167 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | return bp::return_internal_reference<owner_arg>::postcall(args_, result); |
| 168 | // If owner_arg doesn't exist, then Python will have to manage the result lifetime | ||
| 169 |
1/2✓ Branch 1 taken 22 times.
✗ Branch 2 not taken.
|
22 | bp::extract<GeometryModel *> geom_model_extract(result); |
| 170 |
1/2✓ Branch 1 taken 22 times.
✗ Branch 2 not taken.
|
22 | if (geom_model_extract.check()) |
| 171 | { | ||
| 172 | ✗ | return bp::to_python_indirect<GeometryModel, bp::detail::make_owning_holder>()( | |
| 173 |
2/4✓ Branch 1 taken 22 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 22 times.
✗ Branch 5 not taken.
|
22 | geom_model_extract()); |
| 174 | } | ||
| 175 | // If returned value is not a GeometryModel*, then raise an error | ||
| 176 | ✗ | PyErr_SetString( | |
| 177 | PyExc_RuntimeError, | ||
| 178 | "pinocchio::python::return_value_policy only works on GeometryModel* data type"); | ||
| 179 | ✗ | return 0; | |
| 180 | } | ||
| 181 | }; | ||
| 182 | |||
| 183 | template<typename F> | ||
| 184 | 40 | void defBuildUrdf(const char * name, F f, const char * urdf_arg, const char * urdf_doc) | |
| 185 | { | ||
| 186 |
1/2✓ Branch 1 taken 40 times.
✗ Branch 2 not taken.
|
40 | std::ostringstream doc; |
| 187 | doc << "Parse the URDF file given as input looking for the geometry of the given input model " | ||
| 188 | "and\n" | ||
| 189 | "and store either the collision geometries (GeometryType.COLLISION) or the visual " | ||
| 190 | "geometries (GeometryType.VISUAL) in a GeometryModel object.\n" | ||
| 191 | "Parameters:\n" | ||
| 192 | "\tmodel: model of the robot\n" | ||
| 193 | "\n" | ||
| 194 | << urdf_arg << ": " << urdf_doc | ||
| 195 |
5/10✓ Branch 1 taken 40 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 40 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 40 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 40 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 40 times.
✗ Branch 14 not taken.
|
40 | << "\n" |
| 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" | ||
| 202 | "Retuns:\n" | ||
| 203 | "\ta new GeometryModel if `geometry_model` is None else `geometry_model` (that has " | ||
| 204 | "been updated).\n"; | ||
| 205 | |||
| 206 |
3/6✓ Branch 1 taken 40 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 40 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 40 times.
✗ Branch 8 not taken.
|
120 | bp::def( |
| 207 | name, f, | ||
| 208 |
2/4✓ Branch 1 taken 40 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 40 times.
✗ Branch 5 not taken.
|
120 | (bp::arg("model"), bp::arg(urdf_arg), bp::arg("geom_type"), |
| 209 |
3/6✓ Branch 1 taken 40 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 40 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 40 times.
✗ Branch 8 not taken.
|
80 | bp::arg("geometry_model") = static_cast<GeometryModel *>(NULL), |
| 210 |
8/16✓ Branch 1 taken 40 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 40 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 40 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 40 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 40 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 40 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 40 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 40 times.
✗ Branch 23 not taken.
|
80 | bp::arg("package_dirs") = bp::object(), bp::arg("mesh_loader") = bp::object()), |
| 211 |
2/4✓ Branch 1 taken 40 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 40 times.
✗ Branch 6 not taken.
|
80 | doc.str().c_str(), return_value_policy<4>()); |
| 212 | 40 | } | |
| 213 | |||
| 214 | #endif | ||
| 215 | |||
| 216 | 20 | void exposeURDFGeometry() | |
| 217 | { | ||
| 218 | #ifdef PINOCCHIO_WITH_URDFDOM | ||
| 219 | 20 | defBuildUrdf( | |
| 220 | "buildGeomFromUrdf", buildGeomFromUrdfFile, "urdf_filename", | ||
| 221 | "path to the URDF file containing the model of the robot"); | ||
| 222 | 20 | defBuildUrdf( | |
| 223 | "buildGeomFromUrdfString", buildGeomFromUrdfString, "urdf_string", | ||
| 224 | "a string containing the URDF model of the robot"); | ||
| 225 | #endif // #ifdef PINOCCHIO_WITH_URDFDOM | ||
| 226 | 20 | } | |
| 227 | } // namespace python | ||
| 228 | } // namespace pinocchio | ||
| 229 |