Directory: | ./ |
---|---|
File: | src/parsers/urdf/geometry.cpp |
Date: | 2024-08-27 18:20:05 |
Exec | Total | Coverage | |
---|---|---|---|
Lines: | 157 | 174 | 90.2% |
Branches: | 201 | 403 | 49.9% |
Line | Branch | Exec | Source |
---|---|---|---|
1 | // | ||
2 | // Copyright (c) 2015-2020 CNRS INRIA | ||
3 | // | ||
4 | |||
5 | #include "pinocchio/parsers/urdf.hpp" | ||
6 | #include "pinocchio/parsers/urdf/types.hpp" | ||
7 | #include "pinocchio/parsers/urdf/utils.hpp" | ||
8 | #include "pinocchio/parsers/utils.hpp" | ||
9 | |||
10 | #include <boost/property_tree/xml_parser.hpp> | ||
11 | #include <boost/property_tree/ptree.hpp> | ||
12 | |||
13 | #include <urdf_model/model.h> | ||
14 | #include <urdf_parser/urdf_parser.h> | ||
15 | |||
16 | #ifdef PINOCCHIO_WITH_HPP_FCL | ||
17 | |||
18 | #include <hpp/fcl/mesh_loader/loader.h> | ||
19 | #include <hpp/fcl/mesh_loader/assimp.h> | ||
20 | |||
21 | #endif // PINOCCHIO_WITH_HPP_FCL | ||
22 | |||
23 | namespace pinocchio | ||
24 | { | ||
25 | namespace urdf | ||
26 | { | ||
27 | /// \internal | ||
28 | namespace details | ||
29 | { | ||
30 | struct UrdfTree | ||
31 | { | ||
32 | typedef boost::property_tree::ptree ptree; | ||
33 | typedef std::map<std::string, const ptree &> LinkMap_t; | ||
34 | |||
35 | 49 | void parse(const std::string & xmlStr) | |
36 | { | ||
37 |
1/2✓ Branch 1 taken 49 times.
✗ Branch 2 not taken.
|
49 | urdf_ = ::urdf::parseURDF(xmlStr); |
38 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 49 times.
|
49 | if (!urdf_) |
39 | { | ||
40 | ✗ | throw std::invalid_argument("Unable to parse URDF"); | |
41 | } | ||
42 | |||
43 |
1/2✓ Branch 1 taken 49 times.
✗ Branch 2 not taken.
|
49 | std::istringstream iss(xmlStr); |
44 | using namespace boost::property_tree; | ||
45 |
1/2✓ Branch 1 taken 49 times.
✗ Branch 2 not taken.
|
49 | read_xml(iss, tree_, xml_parser::no_comments); |
46 | |||
47 |
20/34✓ Branch 1 taken 49 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 49 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 49 times.
✗ Branch 8 not taken.
✓ Branch 11 taken 49 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 49 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 49 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 49 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 49 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 5249 times.
✗ Branch 27 not taken.
✓ Branch 29 taken 5249 times.
✗ Branch 30 not taken.
✓ Branch 31 taken 5249 times.
✓ Branch 32 taken 5249 times.
✓ Branch 33 taken 5249 times.
✗ Branch 34 not taken.
✓ Branch 36 taken 5249 times.
✗ Branch 37 not taken.
✓ Branch 38 taken 5298 times.
✗ Branch 39 not taken.
✓ Branch 41 taken 5298 times.
✗ Branch 42 not taken.
✓ Branch 43 taken 5249 times.
✓ Branch 44 taken 49 times.
✓ Branch 45 taken 5249 times.
✓ Branch 46 taken 49 times.
|
10547 | BOOST_FOREACH (const ptree::value_type & link, tree_.get_child("robot")) |
48 | { | ||
49 |
2/2✓ Branch 1 taken 2040 times.
✓ Branch 2 taken 3209 times.
|
5249 | if (link.first == "link") |
50 | { | ||
51 |
2/4✓ Branch 1 taken 2040 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2040 times.
✗ Branch 5 not taken.
|
2040 | std::string name = link.second.get<std::string>("<xmlattr>.name"); |
52 |
2/4✓ Branch 1 taken 2040 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2040 times.
✗ Branch 5 not taken.
|
2040 | links_.insert(std::pair<std::string, const ptree &>(name, link.second)); |
53 | 2040 | } | |
54 | } // BOOST_FOREACH | ||
55 | 49 | } | |
56 | |||
57 | 31 | bool isCapsule(const std::string & linkName, const std::string & geomName) const | |
58 | { | ||
59 |
1/2✓ Branch 1 taken 31 times.
✗ Branch 2 not taken.
|
31 | LinkMap_t::const_iterator _link = links_.find(linkName); |
60 |
1/2✗ Branch 2 not taken.
✓ Branch 3 taken 31 times.
|
31 | assert(_link != links_.end()); |
61 | 31 | const ptree & link = _link->second; | |
62 |
4/6✓ Branch 2 taken 31 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 31 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 28 times.
✓ Branch 10 taken 3 times.
|
31 | if (link.count("collision_checking") == 0) |
63 | 28 | return false; | |
64 |
15/34✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3 times.
✗ Branch 8 not taken.
✓ Branch 11 taken 3 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 3 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 3 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 3 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 3 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 3 times.
✗ Branch 27 not taken.
✓ Branch 29 taken 3 times.
✗ Branch 30 not taken.
✓ Branch 31 taken 3 times.
✗ Branch 32 not taken.
✗ Branch 33 not taken.
✗ Branch 34 not taken.
✗ Branch 36 not taken.
✗ Branch 37 not taken.
✓ Branch 38 taken 3 times.
✗ Branch 39 not taken.
✓ Branch 41 taken 3 times.
✗ Branch 42 not taken.
✓ Branch 43 taken 3 times.
✗ Branch 44 not taken.
✓ Branch 45 taken 3 times.
✗ Branch 46 not taken.
|
3 | BOOST_FOREACH (const ptree::value_type & cc, link.get_child("collision_checking")) |
65 | { | ||
66 |
1/2✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
|
3 | if (cc.first == "capsule") |
67 | { | ||
68 | #ifdef PINOCCHIO_URDFDOM_COLLISION_WITH_GROUP_NAME | ||
69 | std::cerr << "Warning: support for tag link/collision_checking/capsule" | ||
70 | " is not available for URDFDOM < 0.3.0" | ||
71 | << std::endl; | ||
72 | #else | ||
73 |
2/4✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
|
3 | std::string name = cc.second.get<std::string>("<xmlattr>.name"); |
74 |
1/2✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
|
3 | if (geomName == name) |
75 | 3 | return true; | |
76 | #endif | ||
77 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 3 times.
|
3 | } |
78 | } // BOOST_FOREACH | ||
79 | |||
80 | ✗ | return false; | |
81 | } | ||
82 | |||
83 | 942 | bool isMeshConvex(const std::string & linkName, const std::string & geomName) const | |
84 | { | ||
85 |
1/2✓ Branch 1 taken 942 times.
✗ Branch 2 not taken.
|
942 | LinkMap_t::const_iterator _link = links_.find(linkName); |
86 |
1/2✗ Branch 2 not taken.
✓ Branch 3 taken 942 times.
|
942 | assert(_link != links_.end()); |
87 | 942 | const ptree & link = _link->second; | |
88 |
4/6✓ Branch 2 taken 942 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 942 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 939 times.
✓ Branch 10 taken 3 times.
|
942 | if (link.count("collision_checking") == 0) |
89 | 939 | return false; | |
90 |
18/34✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3 times.
✗ Branch 8 not taken.
✓ Branch 11 taken 3 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 3 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 3 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 3 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 3 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 6 times.
✗ Branch 27 not taken.
✓ Branch 29 taken 6 times.
✗ Branch 30 not taken.
✓ Branch 31 taken 6 times.
✓ Branch 32 taken 3 times.
✓ Branch 33 taken 3 times.
✗ Branch 34 not taken.
✓ Branch 36 taken 3 times.
✗ Branch 37 not taken.
✓ Branch 38 taken 6 times.
✗ Branch 39 not taken.
✓ Branch 41 taken 6 times.
✗ Branch 42 not taken.
✓ Branch 43 taken 6 times.
✗ Branch 44 not taken.
✓ Branch 45 taken 6 times.
✗ Branch 46 not taken.
|
9 | BOOST_FOREACH (const ptree::value_type & cc, link.get_child("collision_checking")) |
91 | { | ||
92 |
2/2✓ Branch 1 taken 3 times.
✓ Branch 2 taken 3 times.
|
6 | if (cc.first == "convex") |
93 | { | ||
94 | #ifdef PINOCCHIO_URDFDOM_COLLISION_WITH_GROUP_NAME | ||
95 | std::cerr << "Warning: support for tag link/collision_checking/convex" | ||
96 | " is not available for URDFDOM < 0.3.0" | ||
97 | << std::endl; | ||
98 | #else | ||
99 |
2/4✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
|
3 | std::string name = cc.second.get<std::string>("<xmlattr>.name"); |
100 |
1/2✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
|
3 | if (geomName == name) |
101 | 3 | return true; | |
102 | #endif | ||
103 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 3 times.
|
3 | } |
104 | } // BOOST_FOREACH | ||
105 | |||
106 | ✗ | return false; | |
107 | } | ||
108 | |||
109 | // For standard URDF tags | ||
110 | ::urdf::ModelInterfaceSharedPtr urdf_; | ||
111 | // For other tags | ||
112 | ptree tree_; | ||
113 | // A mapping from link name to corresponding child of tree_ | ||
114 | LinkMap_t links_; | ||
115 | }; | ||
116 | |||
117 | template<typename Vector3> | ||
118 | 942 | static void retrieveMeshScale( | |
119 | const ::urdf::MeshSharedPtr & mesh, const Eigen::MatrixBase<Vector3> & scale) | ||
120 | { | ||
121 | 942 | Vector3 & scale_ = PINOCCHIO_EIGEN_CONST_CAST(Vector3, scale); | |
122 |
2/4✓ Branch 4 taken 942 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 942 times.
✗ Branch 9 not taken.
|
942 | scale_ << mesh->scale.x, mesh->scale.y, mesh->scale.z; |
123 | 942 | } | |
124 | |||
125 | #ifdef PINOCCHIO_WITH_HPP_FCL | ||
126 | /** | ||
127 | * @brief Get a fcl::CollisionObject from a URDF geometry, searching | ||
128 | * for it in specified package directories | ||
129 | * | ||
130 | * @param[in] urdf_geometry A shared pointer on the input urdf Geometry | ||
131 | * @param[in] package_dirs A vector containing the different directories where to search | ||
132 | * for packages | ||
133 | * @param[out] meshPath The Absolute path of the mesh currently read | ||
134 | * @param[out] meshScale Scale of transformation currently applied to the mesh | ||
135 | * | ||
136 | * @return A shared pointer on the geometry converted as a fcl::CollisionGeometry | ||
137 | */ | ||
138 | 993 | std::shared_ptr<fcl::CollisionGeometry> inline retrieveCollisionGeometry( | |
139 | const UrdfTree & tree, | ||
140 | fcl::MeshLoaderPtr & meshLoader, | ||
141 | const std::string & linkName, | ||
142 | const std::string & geomName, | ||
143 | const ::urdf::GeometrySharedPtr urdf_geometry, | ||
144 | const std::vector<std::string> & package_dirs, | ||
145 | std::string & meshPath, | ||
146 | Eigen::Vector3d & meshScale) | ||
147 | { | ||
148 | 993 | std::shared_ptr<fcl::CollisionGeometry> geometry; | |
149 | |||
150 | // Handle the case where collision geometry is a mesh | ||
151 |
2/2✓ Branch 1 taken 942 times.
✓ Branch 2 taken 51 times.
|
993 | if (urdf_geometry->type == ::urdf::Geometry::MESH) |
152 | { | ||
153 | const ::urdf::MeshSharedPtr urdf_mesh = | ||
154 | 942 | ::urdf::static_pointer_cast<::urdf::Mesh>(urdf_geometry); | |
155 |
1/2✓ Branch 2 taken 942 times.
✗ Branch 3 not taken.
|
942 | std::string collisionFilename = urdf_mesh->filename; |
156 | |||
157 |
1/2✓ Branch 1 taken 942 times.
✗ Branch 2 not taken.
|
942 | meshPath = retrieveResourcePath(collisionFilename, package_dirs); |
158 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 942 times.
|
942 | if (meshPath == "") |
159 | { | ||
160 | ✗ | std::stringstream ss; | |
161 | ✗ | ss << "Mesh " << collisionFilename << " could not be found."; | |
162 | ✗ | throw std::invalid_argument(ss.str()); | |
163 | } | ||
164 | |||
165 |
1/2✓ Branch 4 taken 942 times.
✗ Branch 5 not taken.
|
942 | fcl::Vec3f scale = fcl::Vec3f(urdf_mesh->scale.x, urdf_mesh->scale.y, urdf_mesh->scale.z); |
166 | |||
167 |
1/2✓ Branch 1 taken 942 times.
✗ Branch 2 not taken.
|
942 | retrieveMeshScale(urdf_mesh, meshScale); |
168 | |||
169 | // Create FCL mesh by parsing Collada file. | ||
170 |
1/2✓ Branch 2 taken 942 times.
✗ Branch 3 not taken.
|
942 | hpp::fcl::BVHModelPtr_t bvh = meshLoader->load(meshPath, scale); |
171 |
1/2✓ Branch 1 taken 942 times.
✗ Branch 2 not taken.
|
942 | bool convex = tree.isMeshConvex(linkName, geomName); |
172 |
2/2✓ Branch 0 taken 3 times.
✓ Branch 1 taken 939 times.
|
942 | if (convex) |
173 | { | ||
174 |
1/2✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
|
3 | bvh->buildConvexRepresentation(false); |
175 |
1/2✓ Branch 3 taken 3 times.
✗ Branch 4 not taken.
|
9 | geometry = std::shared_ptr<fcl::CollisionGeometry>( |
176 | 9 | bvh->convex.get(), [bvh](...) mutable { bvh->convex.reset(); }); | |
177 | } | ||
178 | else | ||
179 | { | ||
180 |
1/2✓ Branch 3 taken 939 times.
✗ Branch 4 not taken.
|
1878 | geometry = std::shared_ptr<fcl::CollisionGeometry>( |
181 | 1858 | bvh.get(), [bvh](...) mutable { bvh.reset(); }); | |
182 | } | ||
183 | 942 | } | |
184 | |||
185 | // Handle the case where collision geometry is a cylinder | ||
186 | // Use FCL capsules for cylinders | ||
187 |
2/2✓ Branch 1 taken 31 times.
✓ Branch 2 taken 20 times.
|
51 | else if (urdf_geometry->type == ::urdf::Geometry::CYLINDER) |
188 | { | ||
189 |
1/2✓ Branch 1 taken 31 times.
✗ Branch 2 not taken.
|
31 | const bool is_capsule = tree.isCapsule(linkName, geomName); |
190 |
3/6✓ Branch 1 taken 31 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 31 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 31 times.
✗ Branch 8 not taken.
|
31 | meshScale << 1, 1, 1; |
191 | const ::urdf::CylinderSharedPtr collisionGeometry = | ||
192 | 31 | ::urdf::static_pointer_cast<::urdf::Cylinder>(urdf_geometry); | |
193 | |||
194 | 31 | double radius = collisionGeometry->radius; | |
195 | 31 | double length = collisionGeometry->length; | |
196 | |||
197 | // Create fcl capsule geometry. | ||
198 |
2/2✓ Branch 0 taken 3 times.
✓ Branch 1 taken 28 times.
|
31 | if (is_capsule) |
199 | { | ||
200 |
1/2✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
|
3 | meshPath = "CAPSULE"; |
201 |
3/6✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3 times.
✗ Branch 8 not taken.
|
3 | geometry = std::shared_ptr<fcl::CollisionGeometry>(new fcl::Capsule(radius, length)); |
202 | } | ||
203 | else | ||
204 | { | ||
205 |
1/2✓ Branch 1 taken 28 times.
✗ Branch 2 not taken.
|
28 | meshPath = "CYLINDER"; |
206 |
3/6✓ Branch 1 taken 28 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 28 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 28 times.
✗ Branch 8 not taken.
|
28 | geometry = std::shared_ptr<fcl::CollisionGeometry>(new fcl::Cylinder(radius, length)); |
207 | } | ||
208 | 31 | } | |
209 | // Handle the case where collision geometry is a box. | ||
210 |
1/2✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
|
20 | else if (urdf_geometry->type == ::urdf::Geometry::BOX) |
211 | { | ||
212 |
1/2✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
|
20 | meshPath = "BOX"; |
213 |
3/6✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 20 times.
✗ Branch 8 not taken.
|
20 | meshScale << 1, 1, 1; |
214 | const ::urdf::BoxSharedPtr collisionGeometry = | ||
215 | 20 | ::urdf::static_pointer_cast<::urdf::Box>(urdf_geometry); | |
216 | |||
217 | 20 | double x = collisionGeometry->dim.x; | |
218 | 20 | double y = collisionGeometry->dim.y; | |
219 | 20 | double z = collisionGeometry->dim.z; | |
220 | |||
221 |
3/6✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 20 times.
✗ Branch 8 not taken.
|
20 | geometry = std::shared_ptr<fcl::CollisionGeometry>(new fcl::Box(x, y, z)); |
222 | 20 | } | |
223 | // Handle the case where collision geometry is a sphere. | ||
224 | ✗ | else if (urdf_geometry->type == ::urdf::Geometry::SPHERE) | |
225 | { | ||
226 | ✗ | meshPath = "SPHERE"; | |
227 | ✗ | meshScale << 1, 1, 1; | |
228 | const ::urdf::SphereSharedPtr collisionGeometry = | ||
229 | ✗ | ::urdf::static_pointer_cast<::urdf::Sphere>(urdf_geometry); | |
230 | |||
231 | ✗ | double radius = collisionGeometry->radius; | |
232 | |||
233 | ✗ | geometry = std::shared_ptr<fcl::CollisionGeometry>(new fcl::Sphere(radius)); | |
234 | } | ||
235 | else | ||
236 | ✗ | throw std::invalid_argument("Unknown geometry type :"); | |
237 | |||
238 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 993 times.
|
993 | if (!geometry) |
239 | { | ||
240 | ✗ | throw std::invalid_argument("The polyhedron retrieved is empty"); | |
241 | } | ||
242 | |||
243 | 993 | return geometry; | |
244 | } | ||
245 | #endif // PINOCCHIO_WITH_HPP_FCL | ||
246 | |||
247 | /** | ||
248 | * @brief Get the first geometry attached to a link | ||
249 | * | ||
250 | * @param[in] link The URDF link | ||
251 | * | ||
252 | * @return Either the first collision or visual | ||
253 | */ | ||
254 | template<typename T> | ||
255 | inline PINOCCHIO_URDF_SHARED_PTR(const T) | ||
256 | getLinkGeometry(const ::urdf::LinkConstSharedPtr link); | ||
257 | |||
258 | template<> | ||
259 | inline ::urdf::CollisionConstSharedPtr | ||
260 | 1646 | getLinkGeometry<::urdf::Collision>(const ::urdf::LinkConstSharedPtr link) | |
261 | { | ||
262 | 1646 | return link->collision; | |
263 | } | ||
264 | |||
265 | template<> | ||
266 | inline ::urdf::VisualConstSharedPtr | ||
267 | 394 | getLinkGeometry<::urdf::Visual>(const ::urdf::LinkConstSharedPtr link) | |
268 | { | ||
269 | 394 | return link->visual; | |
270 | } | ||
271 | |||
272 | /** | ||
273 | * @brief Get the material values from the link visual object | ||
274 | * | ||
275 | * @param[in] Visual/Collision The Visual or the Collision object. | ||
276 | * @param[out] meshTexturePath The absolute file path containing the texture description. | ||
277 | * @param[out] meshColor The mesh RGBA vector. | ||
278 | * @param[in] package_dirs A vector containing the different directories where to search | ||
279 | * for packages | ||
280 | * | ||
281 | */ | ||
282 | template<typename urdfObject> | ||
283 | inline bool getVisualMaterial( | ||
284 | const PINOCCHIO_URDF_SHARED_PTR(urdfObject) urdf_object, | ||
285 | std::string & meshTexturePath, | ||
286 | Eigen::Vector4d & meshColor, | ||
287 | const std::vector<std::string> & package_dirs); | ||
288 | |||
289 | template<> | ||
290 | 721 | inline bool getVisualMaterial<::urdf::Collision>( | |
291 | const ::urdf::CollisionSharedPtr, | ||
292 | std::string & meshTexturePath, | ||
293 | Eigen::Vector4d & meshColor, | ||
294 | const std::vector<std::string> &) | ||
295 | { | ||
296 |
4/8✓ Branch 1 taken 721 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 721 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 721 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 721 times.
✗ Branch 11 not taken.
|
721 | meshColor << 0.9, 0.9, 0.9, 1.; |
297 | 721 | meshTexturePath = ""; | |
298 | 721 | return false; | |
299 | } | ||
300 | |||
301 | template<> | ||
302 | 272 | inline bool getVisualMaterial<::urdf::Visual>( | |
303 | const ::urdf::VisualSharedPtr urdf_visual, | ||
304 | std::string & meshTexturePath, | ||
305 | Eigen::Vector4d & meshColor, | ||
306 | const std::vector<std::string> & package_dirs) | ||
307 | { | ||
308 |
4/8✓ Branch 1 taken 272 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 272 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 272 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 272 times.
✗ Branch 11 not taken.
|
272 | meshColor << 0.9, 0.9, 0.9, 1.; |
309 | 272 | meshTexturePath = ""; | |
310 | 272 | bool overrideMaterial = false; | |
311 |
2/2✓ Branch 2 taken 218 times.
✓ Branch 3 taken 54 times.
|
272 | if (urdf_visual->material) |
312 | { | ||
313 | 218 | overrideMaterial = true; | |
314 |
2/4✓ Branch 3 taken 218 times.
✗ Branch 4 not taken.
✓ Branch 8 taken 218 times.
✗ Branch 9 not taken.
|
218 | meshColor << urdf_visual->material->color.r, urdf_visual->material->color.g, |
315 |
2/4✓ Branch 3 taken 218 times.
✗ Branch 4 not taken.
✓ Branch 8 taken 218 times.
✗ Branch 9 not taken.
|
218 | urdf_visual->material->color.b, urdf_visual->material->color.a; |
316 |
1/2✗ Branch 3 not taken.
✓ Branch 4 taken 218 times.
|
218 | if (urdf_visual->material->texture_filename != "") |
317 | meshTexturePath = | ||
318 | ✗ | retrieveResourcePath((urdf_visual)->material->texture_filename, package_dirs); | |
319 | } | ||
320 | 272 | return overrideMaterial; | |
321 | } | ||
322 | |||
323 | /** | ||
324 | * @brief Get the array of geometries attached to a link | ||
325 | * | ||
326 | * @param[in] link The URDF link | ||
327 | * | ||
328 | * @return the array of either collisions or visuals | ||
329 | */ | ||
330 | template<typename T> | ||
331 | inline const std::vector<PINOCCHIO_URDF_SHARED_PTR(T)> & | ||
332 | getLinkGeometryArray(const ::urdf::LinkConstSharedPtr link); | ||
333 | |||
334 | template<> | ||
335 | inline const std::vector<::urdf::CollisionSharedPtr> & | ||
336 | 712 | getLinkGeometryArray<::urdf::Collision>(const ::urdf::LinkConstSharedPtr link) | |
337 | { | ||
338 | 712 | return link->collision_array; | |
339 | } | ||
340 | |||
341 | template<> | ||
342 | inline const std::vector<::urdf::VisualSharedPtr> & | ||
343 | 272 | getLinkGeometryArray<::urdf::Visual>(const ::urdf::LinkConstSharedPtr link) | |
344 | { | ||
345 | 272 | return link->visual_array; | |
346 | } | ||
347 | |||
348 | /** | ||
349 | * @brief Add the geometries attached to a URDF link to a GeometryModel, looking | ||
350 | * either for collisions or visuals | ||
351 | * | ||
352 | * @param[in] tree The URDF kinematic tree | ||
353 | * @param[in] meshLoader The FCL mesh loader to avoid duplications of already loaded | ||
354 | * geometries | ||
355 | * @param[in] link The current URDF link | ||
356 | * @param model The model to which is the GeometryModel associated | ||
357 | * @param geomModel The GeometryModel where the Collision Objects must be added | ||
358 | * @param[in] package_dirs A vector containing the different directories where to search | ||
359 | * for packages | ||
360 | * | ||
361 | */ | ||
362 | template<typename GeometryType> | ||
363 | 4080 | static void addLinkGeometryToGeomModel( | |
364 | const UrdfTree & tree, | ||
365 | ::hpp::fcl::MeshLoaderPtr & meshLoader, | ||
366 | ::urdf::LinkConstSharedPtr link, | ||
367 | UrdfGeomVisitorBase & visitor, | ||
368 | GeometryModel & geomModel, | ||
369 | const std::vector<std::string> & package_dirs) | ||
370 | { | ||
371 | #ifndef PINOCCHIO_WITH_HPP_FCL | ||
372 | PINOCCHIO_UNUSED_VARIABLE(tree); | ||
373 | PINOCCHIO_UNUSED_VARIABLE(meshLoader); | ||
374 | #endif // PINOCCHIO_WITH_HPP_FCL | ||
375 | |||
376 | typedef std::vector<PINOCCHIO_URDF_SHARED_PTR(GeometryType)> VectorSharedT; | ||
377 | typedef GeometryModel::SE3 SE3; | ||
378 | |||
379 |
2/2✓ Branch 5 taken 984 times.
✓ Branch 6 taken 1056 times.
|
4080 | if (getLinkGeometry<GeometryType>(link)) |
380 | { | ||
381 |
1/2✓ Branch 2 taken 984 times.
✗ Branch 3 not taken.
|
1968 | std::string meshPath = ""; |
382 | |||
383 |
2/4✓ Branch 1 taken 984 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 984 times.
✗ Branch 5 not taken.
|
1968 | Eigen::Vector3d meshScale(Eigen::Vector3d::Ones()); |
384 | |||
385 | 1968 | const std::string & link_name = link->name; | |
386 | |||
387 |
1/2✓ Branch 3 taken 984 times.
✗ Branch 4 not taken.
|
1968 | VectorSharedT geometries_array = getLinkGeometryArray<GeometryType>(link); |
388 | |||
389 | FrameIndex frame_id; | ||
390 |
1/2✓ Branch 1 taken 984 times.
✗ Branch 2 not taken.
|
1968 | UrdfGeomVisitorBase::Frame frame = visitor.getBodyFrame(link_name, frame_id); |
391 |
1/2✓ Branch 1 taken 984 times.
✗ Branch 2 not taken.
|
1968 | const SE3 & body_placement = frame.placement.cast<SE3::Scalar>(); |
392 | |||
393 | 1968 | std::size_t objectId = 0; | |
394 | 3954 | for (typename VectorSharedT::const_iterator i = geometries_array.begin(); | |
395 |
2/2✓ Branch 3 taken 993 times.
✓ Branch 4 taken 984 times.
|
3954 | i != geometries_array.end(); ++i) |
396 | { | ||
397 | 1986 | meshPath.clear(); | |
398 | #ifdef PINOCCHIO_WITH_HPP_FCL | ||
399 | |||
400 | #ifdef PINOCCHIO_URDFDOM_COLLISION_WITH_GROUP_NAME | ||
401 | const std::string & geom_name = (*i)->group_name; | ||
402 | #else | ||
403 | 1986 | const std::string & geom_name = (*i)->name; | |
404 | #endif // PINOCCHIO_URDFDOM_COLLISION_WITH_GROUP_NAME | ||
405 |
1/2✓ Branch 1 taken 993 times.
✗ Branch 2 not taken.
|
1986 | const GeometryObject::CollisionGeometryPtr geometry = retrieveCollisionGeometry( |
406 | 1986 | tree, meshLoader, link_name, geom_name, (*i)->geometry, package_dirs, meshPath, | |
407 | meshScale); | ||
408 | #else | ||
409 | ::urdf::MeshSharedPtr urdf_mesh = | ||
410 | ::urdf::dynamic_pointer_cast<::urdf::Mesh>((*i)->geometry); | ||
411 | if (urdf_mesh) | ||
412 | { | ||
413 | meshPath = retrieveResourcePath(urdf_mesh->filename, package_dirs); | ||
414 | retrieveMeshScale(urdf_mesh, meshScale); | ||
415 | } | ||
416 | |||
417 | const std::shared_ptr<fcl::CollisionGeometry> geometry(new fcl::CollisionGeometry()); | ||
418 | #endif // PINOCCHIO_WITH_HPP_FCL | ||
419 | |||
420 |
1/2✓ Branch 1 taken 993 times.
✗ Branch 2 not taken.
|
1986 | Eigen::Vector4d meshColor; |
421 | 1986 | std::string meshTexturePath; | |
422 | bool overrideMaterial = | ||
423 |
1/2✓ Branch 3 taken 993 times.
✗ Branch 4 not taken.
|
1986 | getVisualMaterial<GeometryType>((*i), meshTexturePath, meshColor, package_dirs); |
424 | |||
425 |
2/4✓ Branch 3 taken 993 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 993 times.
✗ Branch 7 not taken.
|
1986 | const SE3 geomPlacement = body_placement * convertFromUrdf((*i)->origin); |
426 |
1/2✓ Branch 1 taken 993 times.
✗ Branch 2 not taken.
|
1986 | std::ostringstream geometry_object_suffix; |
427 |
2/4✓ Branch 1 taken 993 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 993 times.
✗ Branch 5 not taken.
|
1986 | geometry_object_suffix << "_" << objectId; |
428 |
2/4✓ Branch 1 taken 993 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 993 times.
✗ Branch 5 not taken.
|
1986 | const std::string & geometry_object_name = |
429 | std::string(link_name + geometry_object_suffix.str()); | ||
430 |
2/4✓ Branch 1 taken 993 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 993 times.
✗ Branch 5 not taken.
|
1986 | GeometryObject geometry_object( |
431 | geometry_object_name, frame.parentJoint, frame_id, geomPlacement, geometry, meshPath, | ||
432 | meshScale, overrideMaterial, meshColor, meshTexturePath); | ||
433 |
1/2✓ Branch 1 taken 993 times.
✗ Branch 2 not taken.
|
1986 | geomModel.addGeometryObject(geometry_object); |
434 | 1986 | ++objectId; | |
435 | } | ||
436 | 1968 | } | |
437 | 4080 | } | |
438 | |||
439 | /** | ||
440 | * @brief Recursive procedure for reading the URDF tree, looking for geometries | ||
441 | * This function fill the geometric model with geometry objects retrieved from the | ||
442 | * URDF tree | ||
443 | * | ||
444 | * @param[in] tree The URDF kinematic tree | ||
445 | * @param[in] meshLoader The FCL mesh loader to avoid duplications of already loaded | ||
446 | * geometries | ||
447 | * @param[in] link The current URDF link | ||
448 | * @param model The model to which is the GeometryModel associated | ||
449 | * @param geomModel The GeometryModel where the Collision Objects must be added | ||
450 | * @param[in] package_dirs A vector containing the different directories where to search | ||
451 | * for packages | ||
452 | * @param[in] type The type of objects that must be loaded ( can be VISUAL or | ||
453 | * COLLISION) | ||
454 | * | ||
455 | */ | ||
456 | 2040 | void recursiveParseTreeForGeom( | |
457 | const UrdfTree & tree, | ||
458 | ::hpp::fcl::MeshLoaderPtr & meshLoader, | ||
459 | ::urdf::LinkConstSharedPtr link, | ||
460 | UrdfGeomVisitorBase & visitor, | ||
461 | GeometryModel & geomModel, | ||
462 | const std::vector<std::string> & package_dirs, | ||
463 | const GeometryType type) | ||
464 | { | ||
465 | |||
466 |
2/3✓ Branch 0 taken 1646 times.
✓ Branch 1 taken 394 times.
✗ Branch 2 not taken.
|
2040 | switch (type) |
467 | { | ||
468 | 1646 | case COLLISION: | |
469 |
1/2✓ Branch 2 taken 1646 times.
✗ Branch 3 not taken.
|
1646 | addLinkGeometryToGeomModel<::urdf::Collision>( |
470 | tree, meshLoader, link, visitor, geomModel, package_dirs); | ||
471 | 1646 | break; | |
472 | 394 | case VISUAL: | |
473 |
1/2✓ Branch 2 taken 394 times.
✗ Branch 3 not taken.
|
394 | addLinkGeometryToGeomModel<::urdf::Visual>( |
474 | tree, meshLoader, link, visitor, geomModel, package_dirs); | ||
475 | 394 | break; | |
476 | ✗ | default: | |
477 | ✗ | break; | |
478 | } | ||
479 | |||
480 |
18/30✓ Branch 2 taken 2040 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2040 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 2040 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 2040 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 2040 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 2040 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1991 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 1991 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 1991 times.
✓ Branch 27 taken 1991 times.
✓ Branch 28 taken 1991 times.
✗ Branch 29 not taken.
✓ Branch 31 taken 1991 times.
✗ Branch 32 not taken.
✓ Branch 33 taken 4031 times.
✗ Branch 34 not taken.
✓ Branch 36 taken 4031 times.
✗ Branch 37 not taken.
✓ Branch 38 taken 1991 times.
✓ Branch 39 taken 2040 times.
✓ Branch 40 taken 1991 times.
✓ Branch 41 taken 2040 times.
|
6022 | BOOST_FOREACH (::urdf::LinkConstSharedPtr child, link->child_links) |
481 | { | ||
482 |
1/2✓ Branch 2 taken 1991 times.
✗ Branch 3 not taken.
|
1991 | recursiveParseTreeForGeom( |
483 | tree, meshLoader, child, visitor, geomModel, package_dirs, type); | ||
484 | 1991 | } | |
485 | 2040 | } | |
486 | |||
487 | 49 | void parseTreeForGeom( | |
488 | UrdfGeomVisitorBase & visitor, | ||
489 | const std::istream & xmlStream, | ||
490 | const GeometryType type, | ||
491 | GeometryModel & geomModel, | ||
492 | const std::vector<std::string> & package_dirs, | ||
493 | ::hpp::fcl::MeshLoaderPtr meshLoader) | ||
494 | { | ||
495 | 49 | std::string xmlStr; | |
496 | { | ||
497 |
1/2✓ Branch 1 taken 49 times.
✗ Branch 2 not taken.
|
49 | std::ostringstream os; |
498 |
2/4✓ Branch 1 taken 49 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 49 times.
✗ Branch 5 not taken.
|
49 | os << xmlStream.rdbuf(); |
499 |
1/2✓ Branch 1 taken 49 times.
✗ Branch 2 not taken.
|
49 | xmlStr = os.str(); |
500 | 49 | } | |
501 | |||
502 |
1/2✓ Branch 1 taken 49 times.
✗ Branch 2 not taken.
|
49 | details::UrdfTree tree; |
503 |
1/2✓ Branch 1 taken 49 times.
✗ Branch 2 not taken.
|
49 | tree.parse(xmlStr); |
504 | |||
505 |
1/2✓ Branch 1 taken 49 times.
✗ Branch 2 not taken.
|
49 | std::vector<std::string> hint_directories(package_dirs); |
506 | |||
507 | // Append the ROS_PACKAGE_PATH | ||
508 |
1/2✓ Branch 1 taken 49 times.
✗ Branch 2 not taken.
|
49 | std::vector<std::string> ros_pkg_paths = rosPaths(); |
509 |
1/2✓ Branch 5 taken 49 times.
✗ Branch 6 not taken.
|
49 | hint_directories.insert(hint_directories.end(), ros_pkg_paths.begin(), ros_pkg_paths.end()); |
510 | |||
511 | #ifdef PINOCCHIO_WITH_HPP_FCL | ||
512 |
1/2✓ Branch 1 taken 49 times.
✗ Branch 2 not taken.
|
49 | if (!meshLoader) |
513 |
2/4✓ Branch 1 taken 49 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 49 times.
✗ Branch 6 not taken.
|
49 | meshLoader = fcl::MeshLoaderPtr(new fcl::MeshLoader); |
514 | #endif // ifdef PINOCCHIO_WITH_HPP_FCL | ||
515 | |||
516 |
1/2✓ Branch 1 taken 49 times.
✗ Branch 2 not taken.
|
49 | recursiveParseTreeForGeom( |
517 | 98 | tree, meshLoader, tree.urdf_->getRoot(), visitor, geomModel, hint_directories, type); | |
518 | 49 | } | |
519 | } // namespace details | ||
520 | /// \endinternal | ||
521 | } // namespace urdf | ||
522 | } // namespace pinocchio | ||
523 |