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 |