Directory: | ./ |
---|---|
File: | src/parsers/mjcf/mjcf-graph-geom.cpp |
Date: | 2024-08-27 18:20:05 |
Exec | Total | Coverage | |
---|---|---|---|
Lines: | 214 | 272 | 78.7% |
Branches: | 334 | 772 | 43.3% |
Line | Branch | Exec | Source |
---|---|---|---|
1 | // | ||
2 | // Copyright (c) 2016-2024 CNRS INRIA | ||
3 | // | ||
4 | |||
5 | #include "pinocchio/parsers/mjcf/mjcf-graph.hpp" | ||
6 | #ifdef PINOCCHIO_WITH_HPP_FCL | ||
7 | |||
8 | #include <hpp/fcl/mesh_loader/loader.h> | ||
9 | #include <hpp/fcl/mesh_loader/assimp.h> | ||
10 | |||
11 | #endif // PINOCCHIO_WITH_HPP_FCL | ||
12 | |||
13 | namespace pinocchio | ||
14 | { | ||
15 | namespace mjcf | ||
16 | { | ||
17 | namespace details | ||
18 | { | ||
19 | 25 | static double computeVolume(const Eigen::VectorXd & size, const std::string & geomType) | |
20 | { | ||
21 | 25 | double pi = boost::math::constants::pi<double>(); | |
22 |
2/2✓ Branch 1 taken 6 times.
✓ Branch 2 taken 19 times.
|
25 | if (geomType == "box") |
23 | { | ||
24 | 6 | return size.prod(); | |
25 | } | ||
26 |
2/2✓ Branch 1 taken 6 times.
✓ Branch 2 taken 13 times.
|
19 | else if (geomType == "cylinder") |
27 | { | ||
28 | 6 | return 2 * pi * std::pow(size(0), 2) * size(1); | |
29 | } | ||
30 |
2/2✓ Branch 1 taken 3 times.
✓ Branch 2 taken 10 times.
|
13 | else if (geomType == "sphere") |
31 | { | ||
32 | 3 | return 4.0 / 3 * pi * std::pow(size(0), 3); | |
33 | } | ||
34 |
2/2✓ Branch 1 taken 6 times.
✓ Branch 2 taken 4 times.
|
10 | else if (geomType == "capsule") |
35 | { | ||
36 | 6 | return 4.0 / 3 * pi * std::pow(size(0), 3) + 2 * pi * std::pow(size(0), 2) * size(1); | |
37 | } | ||
38 |
1/2✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
|
4 | else if (geomType == "ellipsoid") |
39 | { | ||
40 | 4 | return 4.0 / 3 * pi * size.prod(); | |
41 | } | ||
42 | else | ||
43 | { | ||
44 | ✗ | throw std::invalid_argument("geometry type does not exist"); | |
45 | } | ||
46 | } | ||
47 | |||
48 | 5 | bool isType(const MjcfGeom & geom, const GeometryType & type) | |
49 | { | ||
50 |
1/2✓ Branch 0 taken 5 times.
✗ Branch 1 not taken.
|
5 | switch (type) |
51 | { | ||
52 | 5 | case ::pinocchio::COLLISION: | |
53 | 5 | return geom.geomKind != MjcfGeom::VISUAL; | |
54 | ✗ | default: | |
55 | case ::pinocchio::VISUAL: | ||
56 | ✗ | return geom.geomKind != MjcfGeom::COLLISION; | |
57 | } | ||
58 | } | ||
59 | |||
60 | /** | ||
61 | * @brief Get a fcl::CollisionObject from an mjcf geometry, searching | ||
62 | * for it in specified package directories | ||
63 | * | ||
64 | * @param[in] geom A mjcf geometry object | ||
65 | * @return A shared pointer on the geometry converted as a fcl::CollisionGeometry | ||
66 | */ | ||
67 | #ifdef PINOCCHIO_WITH_HPP_FCL | ||
68 | 5 | static std::shared_ptr<fcl::CollisionGeometry> retrieveCollisionGeometry( | |
69 | ::hpp::fcl::MeshLoaderPtr & meshLoader, | ||
70 | const MjcfGeom & geom, | ||
71 | const MjcfGraph & currentGraph, | ||
72 | std::string & meshPath, | ||
73 | Eigen::Vector3d & meshScale) | ||
74 | { | ||
75 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 5 times.
|
5 | if (geom.geomType == "mesh") |
76 | { | ||
77 | ✗ | MjcfMesh currentMesh = currentGraph.mapOfMeshes.at(geom.meshName); | |
78 | ✗ | meshPath = currentMesh.filePath; | |
79 | ✗ | meshScale = currentMesh.scale; | |
80 | ✗ | hpp::fcl::BVHModelPtr_t bvh = meshLoader->load(meshPath, meshScale); | |
81 | ✗ | return bvh; | |
82 | } | ||
83 |
2/2✓ Branch 1 taken 1 times.
✓ Branch 2 taken 4 times.
|
5 | else if (geom.geomType == "cylinder") |
84 | { | ||
85 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | meshPath = "CYLINDER"; |
86 |
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 | meshScale << 1, 1, 1; |
87 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | double radius = geom.size(0); |
88 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | double height = geom.size(1) * 2; |
89 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | return std::make_shared<fcl::Cylinder>(radius, height); |
90 | } | ||
91 |
2/2✓ Branch 1 taken 1 times.
✓ Branch 2 taken 3 times.
|
4 | else if (geom.geomType == "box") |
92 | { | ||
93 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | meshPath = "BOX"; |
94 |
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 | meshScale << 1, 1, 1; |
95 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | double x = geom.size(0); |
96 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | double y = geom.size(1); |
97 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | double z = geom.size(2); |
98 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | return std::make_shared<fcl::Box>(x, y, z); |
99 | } | ||
100 |
2/2✓ Branch 1 taken 1 times.
✓ Branch 2 taken 2 times.
|
3 | else if (geom.geomType == "sphere") |
101 | { | ||
102 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | meshPath = "SPHERE"; |
103 |
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 | meshScale << 1, 1, 1; |
104 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | double radius = geom.size(0); |
105 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | return std::make_shared<fcl::Sphere>(radius); |
106 | } | ||
107 |
2/2✓ Branch 1 taken 1 times.
✓ Branch 2 taken 1 times.
|
2 | else if (geom.geomType == "ellipsoid") |
108 | { | ||
109 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | meshPath = "ELLIPSOID"; |
110 |
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 | meshScale << 1, 1, 1; |
111 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | double rx = geom.size(0); |
112 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | double ry = geom.size(1); |
113 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | double rz = geom.size(2); |
114 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | return std::make_shared<fcl::Ellipsoid>(rx, ry, rz); |
115 | } | ||
116 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | else if (geom.geomType == "capsule") |
117 | { | ||
118 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | meshPath = "CAPSULE"; |
119 |
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 | meshScale << 1, 1, 1; |
120 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | double radius = geom.size(0); |
121 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | double height = geom.size(1) * 2; |
122 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | return std::make_shared<fcl::Capsule>(radius, height); |
123 | } | ||
124 | else | ||
125 | ✗ | throw std::invalid_argument("Unknown geometry type :"); // Missing plane, hfield and sdf | |
126 | } | ||
127 | #else | ||
128 | static std::shared_ptr<fcl::CollisionGeometry> retrieveCollisionGeometry( | ||
129 | ::hpp::fcl::MeshLoaderPtr &, | ||
130 | const MjcfGeom &, | ||
131 | const MjcfGraph &, | ||
132 | std::string &, | ||
133 | Eigen::Vector3d &) | ||
134 | { | ||
135 | return std::make_shared<fcl::CollisionGeometry>(); | ||
136 | } | ||
137 | #endif | ||
138 | |||
139 | /// @brief Add all geometries associated with a body | ||
140 | /// @param currentBody Body from which geometries will be collected | ||
141 | /// @param type Type of geometry to parse (COLLISION or VISUAL) | ||
142 | /// @param geomModel geometry model to fill | ||
143 | /// @param meshLoader mesh loader from hpp::fcl | ||
144 | 5 | static void addLinksToGeomModel( | |
145 | const MjcfBody & currentBody, | ||
146 | const MjcfGraph & currentGraph, | ||
147 | GeometryModel & geomModel, | ||
148 | ::hpp::fcl::MeshLoaderPtr & meshLoader, | ||
149 | const GeometryType & type) | ||
150 | { | ||
151 | 5 | const std::string & bodyName = currentBody.bodyName; | |
152 |
1/2✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
|
5 | FrameIndex frame_id = currentGraph.urdfVisitor.getBodyId(bodyName); |
153 |
1/2✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
|
5 | Frame frame = currentGraph.urdfVisitor.getBodyFrame(bodyName); |
154 | |||
155 | 5 | const SE3 & bodyPlacement = frame.placement; | |
156 | |||
157 | 5 | std::size_t objectId = 0; | |
158 |
2/2✓ Branch 5 taken 5 times.
✓ Branch 6 taken 5 times.
|
10 | for (const auto & geom : currentBody.geomChildren) |
159 | { | ||
160 |
1/2✓ Branch 2 taken 5 times.
✗ Branch 3 not taken.
|
5 | std::string meshPath = ""; |
161 |
2/4✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
|
5 | Eigen::Vector3d meshScale(Eigen::Vector3d::Ones()); |
162 |
2/4✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 5 times.
✗ Branch 4 not taken.
|
5 | if (isType(geom, type)) |
163 | { | ||
164 | const GeometryObject::CollisionGeometryPtr geometry = | ||
165 |
1/2✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
|
5 | retrieveCollisionGeometry(meshLoader, geom, currentGraph, meshPath, meshScale); |
166 | |||
167 | 5 | bool overrideMaterial = false; | |
168 |
1/2✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
|
5 | Eigen::Vector4d meshColor = geom.rgba; |
169 | 5 | std::string texturePath; | |
170 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 5 times.
|
5 | if (!geom.materialName.empty()) |
171 | { | ||
172 | ✗ | MjcfMaterial mat = currentGraph.mapOfMaterials.at(geom.materialName); | |
173 | ✗ | meshColor = mat.rgba; | |
174 | ✗ | overrideMaterial = true; | |
175 | ✗ | if (!mat.texture.empty()) | |
176 | { | ||
177 | ✗ | MjcfTexture text = currentGraph.mapOfTextures.at(mat.texture); | |
178 | ✗ | texturePath = text.filePath; | |
179 | } | ||
180 | } | ||
181 |
1/2✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
|
5 | const SE3 geomPlacement = bodyPlacement * geom.geomPlacement; |
182 |
1/2✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
|
5 | std::ostringstream geometry_object_suffix; |
183 |
2/4✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
|
5 | geometry_object_suffix << "_" << objectId; |
184 |
2/4✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
|
5 | std::string geometry_object_name = std::string(bodyName + geometry_object_suffix.str()); |
185 | |||
186 | GeometryObject geometry_object( | ||
187 | geometry_object_name, frame.parentJoint, frame_id, geomPlacement, geometry, meshPath, | ||
188 |
2/4✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
|
5 | meshScale, overrideMaterial, meshColor, texturePath); |
189 |
1/2✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
|
5 | geomModel.addGeometryObject(geometry_object); |
190 | 5 | ++objectId; | |
191 | 5 | } | |
192 | 5 | } | |
193 | 5 | } | |
194 | |||
195 | 1 | void MjcfMaterial::goThroughElement(const ptree & el) | |
196 | { | ||
197 | // texture name | ||
198 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | auto text_s = el.get_optional<std::string>("<xmlattr>.texture"); |
199 |
1/2✓ Branch 0 taken 1 times.
✗ Branch 1 not taken.
|
1 | if (text_s) |
200 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
1 | texture = *text_s; |
201 | |||
202 | // Emission | ||
203 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
1 | auto single_v = el.get_optional<float>("<xmlattr>.emission"); |
204 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 1 times.
|
1 | if (single_v) |
205 | ✗ | emission = *single_v; | |
206 | |||
207 | // Specular | ||
208 |
3/6✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✓ Branch 8 taken 1 times.
|
2 | if ((single_v = el.get_optional<float>("<xmlattr>.specular"))) |
209 | ✗ | specular = *single_v; | |
210 | |||
211 | // Shininess | ||
212 |
3/6✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✓ Branch 8 taken 1 times.
|
2 | if ((single_v = el.get_optional<float>("<xmlattr>.shininess"))) |
213 | ✗ | shininess = *single_v; | |
214 | |||
215 |
3/6✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✓ Branch 8 taken 1 times.
|
2 | if ((single_v = el.get_optional<float>("<xmlattr>.reflectance"))) |
216 | ✗ | reflectance = *single_v; | |
217 | |||
218 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | auto rgba_ = el.get_optional<std::string>("<xmlattr>.rgba"); |
219 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 1 times.
|
1 | if (rgba_) |
220 | ✗ | rgba = internal::getVectorFromStream<4>(*rgba_); | |
221 | 1 | } | |
222 | |||
223 | 27 | void MjcfGeom::goThroughElement(const ptree & el, const MjcfGraph & currentGraph) | |
224 | { | ||
225 |
11/22✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 27 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 17 times.
✓ Branch 7 taken 10 times.
✓ Branch 9 taken 17 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 17 times.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✓ Branch 15 taken 17 times.
✓ Branch 16 taken 17 times.
✓ Branch 17 taken 10 times.
✓ Branch 19 taken 27 times.
✗ Branch 20 not taken.
✗ Branch 22 not taken.
✓ Branch 23 taken 27 times.
✗ Branch 24 not taken.
✗ Branch 25 not taken.
✗ Branch 27 not taken.
✗ Branch 28 not taken.
|
71 | if (el.get_child_optional("<xmlattr>.pos") && el.get_child_optional("<xmlattr>.fromto")) |
226 | ✗ | throw std::invalid_argument("Both pos and fromto are defined in geom object"); | |
227 | |||
228 | // Placement | ||
229 |
2/4✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 27 times.
✗ Branch 5 not taken.
|
27 | geomPlacement = currentGraph.convertPosition(el); |
230 | |||
231 | // density | ||
232 |
2/4✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 27 times.
✗ Branch 5 not taken.
|
27 | auto value_v = el.get_optional<double>("<xmlattr>.density"); |
233 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 27 times.
|
27 | if (value_v) |
234 | ✗ | density = *value_v; | |
235 | |||
236 | // contype | ||
237 |
2/4✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 27 times.
✗ Branch 5 not taken.
|
27 | auto val_v = el.get_optional<int>("<xmlattr>.contype"); |
238 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 27 times.
|
27 | if (val_v) |
239 | ✗ | contype = *val_v; | |
240 | // condim | ||
241 |
3/6✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 27 times.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✓ Branch 8 taken 27 times.
|
54 | if ((val_v = el.get_optional<int>("<xmlattr>.conaffinity"))) |
242 | ✗ | conaffinity = *val_v; | |
243 | // group | ||
244 |
3/6✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 27 times.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✓ Branch 8 taken 27 times.
|
54 | if ((val_v = el.get_optional<int>("<xmlattr>.group"))) |
245 | ✗ | group = *val_v; | |
246 | |||
247 | // mass | ||
248 |
2/4✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 27 times.
✗ Branch 5 not taken.
|
27 | massGeom = el.get_optional<double>("<xmlattr>.mass"); |
249 | |||
250 | // shellinertia | ||
251 |
2/4✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 27 times.
✗ Branch 5 not taken.
|
54 | auto value_s = el.get_optional<std::string>("<xmlattr>.shellinertia"); |
252 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 27 times.
|
27 | if (value_s) |
253 | ✗ | throw std::invalid_argument("Shell inertia computation is not supported yet."); | |
254 | |||
255 | // type | ||
256 |
3/6✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 27 times.
✗ Branch 5 not taken.
✓ Branch 9 taken 27 times.
✗ Branch 10 not taken.
|
54 | if ((value_s = el.get_optional<std::string>("<xmlattr>.type"))) |
257 |
2/4✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 27 times.
✗ Branch 5 not taken.
|
27 | geomType = *value_s; |
258 | |||
259 | // material name | ||
260 |
3/6✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 27 times.
✗ Branch 5 not taken.
✗ Branch 9 not taken.
✓ Branch 10 taken 27 times.
|
54 | if ((value_s = el.get_optional<std::string>("<xmlattr>.material"))) |
261 | ✗ | materialName = *value_s; | |
262 | |||
263 |
2/2✓ Branch 1 taken 2 times.
✓ Branch 2 taken 25 times.
|
27 | if (geomType == "mesh") |
264 | { | ||
265 |
2/4✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
|
2 | value_s = el.get_optional<std::string>("<xmlattr>.mesh"); |
266 |
1/2✓ Branch 0 taken 2 times.
✗ Branch 1 not taken.
|
2 | if (value_s) |
267 |
2/4✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
|
2 | meshName = *value_s; |
268 | } | ||
269 | |||
270 | // rgba | ||
271 |
3/6✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 27 times.
✗ Branch 5 not taken.
✗ Branch 9 not taken.
✓ Branch 10 taken 27 times.
|
54 | if ((value_s = el.get_optional<std::string>("<xmlattr>.rgba"))) |
272 | ✗ | rgba = internal::getVectorFromStream<4>(*value_s); | |
273 | |||
274 | // size | ||
275 |
4/6✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 27 times.
✗ Branch 5 not taken.
✓ Branch 9 taken 25 times.
✓ Branch 10 taken 2 times.
|
54 | if ((value_s = el.get_optional<std::string>("<xmlattr>.size"))) |
276 |
2/4✓ Branch 1 taken 25 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 25 times.
✗ Branch 5 not taken.
|
25 | sizeS = *value_s; |
277 | // fromto | ||
278 |
1/2✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
|
27 | if (!fromtoS) |
279 |
2/4✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 27 times.
✗ Branch 5 not taken.
|
27 | fromtoS = el.get_optional<std::string>("<xmlattr>.fromto"); |
280 |
2/2✓ Branch 1 taken 6 times.
✓ Branch 2 taken 21 times.
|
54 | if (fromtoS) |
281 | { | ||
282 |
3/6✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6 times.
✗ Branch 8 not taken.
|
6 | Eigen::VectorXd poses = internal::getVectorFromStream<6>(*fromtoS); |
283 |
6/12✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 6 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 6 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 6 times.
✗ Branch 17 not taken.
|
6 | geomPlacement.translation() = (poses.head(3) + poses.tail(3)) / 2; |
284 | |||
285 |
4/8✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 6 times.
✗ Branch 11 not taken.
|
6 | Eigen::Vector3d zaxis = poses.tail(3) - poses.head(3); |
286 | // Compute the rotation matrix that maps z_axis to unit z | ||
287 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
6 | geomPlacement.rotation() = |
288 |
3/6✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6 times.
✗ Branch 8 not taken.
|
12 | Eigen::Quaterniond::FromTwoVectors(Eigen::Vector3d::UnitZ(), zaxis).toRotationMatrix(); |
289 | 6 | } | |
290 | 27 | } | |
291 | |||
292 | 27 | void MjcfGeom::findKind() | |
293 | { | ||
294 | // Common mechanism to set visual only geometry | ||
295 |
1/4✗ Branch 0 not taken.
✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
|
27 | if (contype == 0 && conaffinity == 0) |
296 | ✗ | geomKind = VISUAL; | |
297 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 27 times.
|
27 | else if (group > 2) // Mechanism for Collision only geometries |
298 | ✗ | geomKind = COLLISION; | |
299 | else | ||
300 | 27 | geomKind = BOTH; | |
301 | 27 | } | |
302 | |||
303 | 27 | void MjcfGeom::computeSize() | |
304 | { | ||
305 |
5/10✓ Branch 2 taken 27 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 27 times.
✗ Branch 7 not taken.
✓ Branch 10 taken 27 times.
✗ Branch 11 not taken.
✓ Branch 14 taken 27 times.
✗ Branch 15 not taken.
✓ Branch 18 taken 27 times.
✗ Branch 19 not taken.
|
189 | std::vector<std::string> forbiddenListFromTo = {"plane", "hfield", "mesh", "sphere"}; |
306 | 27 | if ( | |
307 | fromtoS | ||
308 |
2/4✓ Branch 3 taken 6 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✓ Branch 6 taken 6 times.
|
33 | && std::find(forbiddenListFromTo.begin(), forbiddenListFromTo.end(), geomType) |
309 |
3/4✓ Branch 1 taken 6 times.
✓ Branch 2 taken 21 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 27 times.
|
87 | != forbiddenListFromTo.end()) |
310 | ✗ | throw std::invalid_argument( | |
311 | ✗ | "fromto tag can only be used with capsules, boxes, cylinders and ellipsoids"); | |
312 | |||
313 | // First deal with cases that do not work with fromto | ||
314 |
2/2✓ Branch 1 taken 3 times.
✓ Branch 2 taken 24 times.
|
27 | if (geomType == "sphere") |
315 |
2/4✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
|
3 | size = internal::getVectorFromStream<1>(sizeS); |
316 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 24 times.
|
24 | else if (geomType == "plane") |
317 | ✗ | size = internal::getVectorFromStream<3>(sizeS); | |
318 |
2/2✓ Branch 1 taken 6 times.
✓ Branch 2 taken 18 times.
|
24 | else if (geomType == "box") |
319 | { | ||
320 |
2/2✓ Branch 1 taken 5 times.
✓ Branch 2 taken 1 times.
|
6 | if (!fromtoS) |
321 |
3/6✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 5 times.
✗ Branch 8 not taken.
|
5 | size = internal::getVectorFromStream<3>(sizeS) * 2; // half x, half y, half z |
322 | else | ||
323 | { | ||
324 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
1 | size = Eigen::Vector3d::Zero(); |
325 |
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 | size(0) = internal::getVectorFromStream<1>(sizeS)(0) * 2; |
326 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
1 | size(1) = size(0); |
327 |
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 | Eigen::VectorXd poses = internal::getVectorFromStream<6>(*fromtoS); |
328 |
4/8✓ 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.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
|
1 | Eigen::Vector3d zaxis = poses.tail(3) - poses.head(3); |
329 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
1 | size(2) = zaxis.norm(); |
330 | 1 | } | |
331 | } | ||
332 |
2/2✓ Branch 1 taken 4 times.
✓ Branch 2 taken 14 times.
|
18 | else if (geomType == "ellipsoid") |
333 | { | ||
334 |
2/2✓ Branch 1 taken 3 times.
✓ Branch 2 taken 1 times.
|
4 | if (!fromtoS) |
335 |
2/4✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
|
3 | size = internal::getVectorFromStream<3>(sizeS); // radius and half length |
336 | else | ||
337 | { | ||
338 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
1 | size = Eigen::Vector3d::Zero(); |
339 |
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 | size(0) = internal::getVectorFromStream<1>(sizeS)(0); |
340 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
1 | size(1) = size(0); |
341 |
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 | Eigen::VectorXd poses = internal::getVectorFromStream<6>(*fromtoS); |
342 |
4/8✓ 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.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
|
1 | Eigen::Vector3d zaxis = poses.tail(3) - poses.head(3); |
343 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
1 | size(2) = zaxis.norm() / 2; // to get radius; |
344 | 1 | } | |
345 | } | ||
346 |
6/6✓ Branch 1 taken 8 times.
✓ Branch 2 taken 6 times.
✓ Branch 4 taken 6 times.
✓ Branch 5 taken 2 times.
✓ Branch 6 taken 12 times.
✓ Branch 7 taken 2 times.
|
14 | else if (geomType == "cylinder" || geomType == "capsule") |
347 | { | ||
348 |
2/2✓ Branch 1 taken 8 times.
✓ Branch 2 taken 4 times.
|
12 | if (!fromtoS) |
349 |
2/4✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
|
8 | size = internal::getVectorFromStream<2>(sizeS); |
350 | else | ||
351 | { | ||
352 |
2/4✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
|
4 | size = Eigen::Vector2d::Zero(); |
353 |
3/6✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4 times.
✗ Branch 8 not taken.
|
4 | size(0) = internal::getVectorFromStream<1>(sizeS)(0); |
354 |
3/6✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4 times.
✗ Branch 8 not taken.
|
4 | Eigen::VectorXd poses = internal::getVectorFromStream<6>(*fromtoS); |
355 |
4/8✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 4 times.
✗ Branch 11 not taken.
|
4 | Eigen::Vector3d zaxis = poses.tail(3) - poses.head(3); |
356 |
2/4✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
|
4 | size(1) = zaxis.norm() / 2; // to get half height |
357 | 4 | } | |
358 | } | ||
359 |
1/2✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
2 | else if (geomType == "mesh") |
360 | 2 | return; | |
361 | else | ||
362 | ✗ | throw std::invalid_argument("geomType does not exist"); | |
363 |
2/2✓ Branch 1 taken 25 times.
✓ Branch 2 taken 2 times.
|
27 | } |
364 | |||
365 | 27 | void MjcfGeom::computeInertia() | |
366 | { | ||
367 |
6/8✓ Branch 1 taken 25 times.
✓ Branch 2 taken 2 times.
✓ Branch 4 taken 25 times.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✓ Branch 8 taken 25 times.
✓ Branch 9 taken 2 times.
✓ Branch 10 taken 25 times.
|
27 | if (geomType == "mesh" || geomType == "plane" || geomType == "hfield") |
368 | 2 | return; | |
369 | |||
370 | double mass; | ||
371 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 25 times.
|
50 | if (massGeom) |
372 | ✗ | mass = *massGeom; | |
373 | else | ||
374 | 25 | mass = computeVolume(size, geomType) * density; | |
375 | |||
376 |
2/2✓ Branch 1 taken 6 times.
✓ Branch 2 taken 19 times.
|
25 | if (geomType == "box") |
377 | { | ||
378 |
1/2✓ Branch 5 taken 6 times.
✗ Branch 6 not taken.
|
6 | geomInertia = Inertia::FromBox(mass, size(0), size(1), size(2)); |
379 | } | ||
380 |
2/2✓ Branch 1 taken 6 times.
✓ Branch 2 taken 13 times.
|
19 | else if (geomType == "cylinder") |
381 | { | ||
382 |
1/2✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
|
6 | geomInertia = Inertia::FromCylinder(mass, size(0), size(1) * 2); |
383 | } | ||
384 |
2/2✓ Branch 1 taken 4 times.
✓ Branch 2 taken 9 times.
|
13 | else if (geomType == "ellipsoid") |
385 | { | ||
386 |
1/2✓ Branch 5 taken 4 times.
✗ Branch 6 not taken.
|
4 | geomInertia = Inertia::FromEllipsoid(mass, size(0), size(1), size(2)); |
387 | } | ||
388 |
2/2✓ Branch 1 taken 3 times.
✓ Branch 2 taken 6 times.
|
9 | else if (geomType == "sphere") |
389 | { | ||
390 |
1/2✓ Branch 3 taken 3 times.
✗ Branch 4 not taken.
|
3 | geomInertia = Inertia::FromSphere(mass, size(0)); |
391 | } | ||
392 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
6 | else if (geomType == "capsule") |
393 | { | ||
394 |
1/2✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
|
6 | geomInertia = Inertia::FromCapsule(mass, size(0), size(1) * 2); |
395 | } | ||
396 | else | ||
397 | { | ||
398 | ✗ | throw std::invalid_argument("Unsupported geometry type"); | |
399 | } | ||
400 | } | ||
401 | |||
402 | void | ||
403 | 27 | MjcfGeom::fill(const ptree & el, const MjcfBody & currentBody, const MjcfGraph & currentGraph) | |
404 | { | ||
405 | // Name | ||
406 |
2/4✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 27 times.
✗ Branch 5 not taken.
|
54 | auto name_s = el.get_optional<std::string>("<xmlattr>.name"); |
407 |
2/2✓ Branch 0 taken 4 times.
✓ Branch 1 taken 23 times.
|
27 | if (name_s) |
408 |
2/4✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
|
4 | geomName = *name_s; |
409 | else | ||
410 | geomName = | ||
411 |
3/6✓ Branch 2 taken 23 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 23 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 23 times.
✗ Branch 9 not taken.
|
23 | currentBody.bodyName + "Geom_" + std::to_string(currentBody.geomChildren.size()); |
412 | |||
413 | // ChildClass < Class < Real Joint | ||
414 | // childClass | ||
415 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 27 times.
|
27 | if (currentBody.childClass != "") |
416 | { | ||
417 | ✗ | const MjcfClass & classE = currentGraph.mapOfClasses.at(currentBody.childClass); | |
418 | ✗ | if (auto geom_p = classE.classElement.get_child_optional("geom")) | |
419 | ✗ | goThroughElement(*geom_p, currentGraph); | |
420 | } | ||
421 | |||
422 | // Class | ||
423 |
2/4✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 27 times.
✗ Branch 5 not taken.
|
54 | auto cl_s = el.get_optional<std::string>("<xmlattr>.class"); |
424 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 27 times.
|
27 | if (cl_s) |
425 | { | ||
426 | ✗ | std::string className = *cl_s; | |
427 | ✗ | const MjcfClass & classE = currentGraph.mapOfClasses.at(className); | |
428 | ✗ | if (auto geom_p = classE.classElement.get_child_optional("geom")) | |
429 | ✗ | goThroughElement(*geom_p, currentGraph); | |
430 | } | ||
431 | |||
432 | // Geom | ||
433 |
1/2✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
|
27 | goThroughElement(el, currentGraph); |
434 |
4/6✓ Branch 1 taken 2 times.
✓ Branch 2 taken 25 times.
✗ Branch 4 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
✓ Branch 7 taken 27 times.
|
27 | if (geomType == "mesh" && meshName.empty()) |
435 | ✗ | throw std::invalid_argument("Type is mesh but no mesh file were provided"); | |
436 | // Compute Kind of geometry | ||
437 |
1/2✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
|
27 | findKind(); |
438 | |||
439 |
1/2✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
|
27 | computeSize(); |
440 | |||
441 | // Compute Mass and inertia of geom object | ||
442 |
1/2✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
|
27 | computeInertia(); |
443 | 27 | } | |
444 | |||
445 | 1 | void MjcfSite::goThroughElement(const ptree & el, const MjcfGraph & currentGraph) | |
446 | { | ||
447 |
9/22✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 1 times.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✓ Branch 15 taken 1 times.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✗ Branch 22 not taken.
✓ Branch 23 taken 1 times.
✗ Branch 24 not taken.
✗ Branch 25 not taken.
✗ Branch 27 not taken.
✗ Branch 28 not taken.
|
3 | if (el.get_child_optional("<xmlattr>.pos") && el.get_child_optional("<xmlattr>.fromto")) |
448 | ✗ | throw std::invalid_argument("Both pos and fromto are defined in site object"); | |
449 | |||
450 | // Placement | ||
451 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
1 | sitePlacement = currentGraph.convertPosition(el); |
452 | |||
453 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | auto fromtoS = el.get_optional<std::string>("<xmlattr>.fromto"); |
454 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 1 times.
|
1 | if (fromtoS) |
455 | { | ||
456 | ✗ | Eigen::VectorXd poses = internal::getVectorFromStream<6>(*fromtoS); | |
457 | ✗ | sitePlacement.translation() = (poses.head(3) + poses.tail(3)) / 2; | |
458 | |||
459 | ✗ | Eigen::Vector3d zaxis = poses.tail(3) - poses.head(3); | |
460 | // Compute the rotation matrix that maps z_axis to unit z | ||
461 | ✗ | sitePlacement.rotation() = | |
462 | ✗ | Eigen::Quaterniond::FromTwoVectors(Eigen::Vector3d::UnitZ(), zaxis).toRotationMatrix(); | |
463 | } | ||
464 | 1 | } | |
465 | |||
466 | void | ||
467 | 1 | MjcfSite::fill(const ptree & el, const MjcfBody & currentBody, const MjcfGraph & currentGraph) | |
468 | { | ||
469 | // Name | ||
470 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | auto name_s = el.get_optional<std::string>("<xmlattr>.name"); |
471 |
1/2✓ Branch 0 taken 1 times.
✗ Branch 1 not taken.
|
1 | if (name_s) |
472 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
1 | siteName = *name_s; |
473 | else | ||
474 | siteName = | ||
475 | ✗ | currentBody.bodyName + "Site_" + std::to_string(currentBody.siteChildren.size()); | |
476 | |||
477 | // ChildClass < Class < Real Joint | ||
478 | // childClass | ||
479 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 1 times.
|
1 | if (currentBody.childClass != "") |
480 | { | ||
481 | ✗ | const MjcfClass & classE = currentGraph.mapOfClasses.at(currentBody.childClass); | |
482 | ✗ | if (auto site_p = classE.classElement.get_child_optional("site")) | |
483 | ✗ | goThroughElement(*site_p, currentGraph); | |
484 | } | ||
485 | |||
486 | // Class | ||
487 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | auto cl_s = el.get_optional<std::string>("<xmlattr>.class"); |
488 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 1 times.
|
1 | if (cl_s) |
489 | { | ||
490 | ✗ | std::string className = *cl_s; | |
491 | ✗ | const MjcfClass & classE = currentGraph.mapOfClasses.at(className); | |
492 | ✗ | if (auto site_p = classE.classElement.get_child_optional("site")) | |
493 | ✗ | goThroughElement(*site_p, currentGraph); | |
494 | } | ||
495 | |||
496 | // Site | ||
497 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | goThroughElement(el, currentGraph); |
498 | 1 | } | |
499 | |||
500 | 1 | void MjcfGraph::parseGeomTree( | |
501 | const GeometryType & type, | ||
502 | GeometryModel & geomModel, | ||
503 | ::hpp::fcl::MeshLoaderPtr & meshLoader) | ||
504 | { | ||
505 | #ifdef PINOCCHIO_WITH_HPP_FCL | ||
506 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | if (!meshLoader) |
507 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
1 | meshLoader = std::make_shared<fcl::MeshLoader>(fcl::MeshLoader()); |
508 | #endif // ifdef PINOCCHIO_WITH_HPP_FCL | ||
509 | |||
510 |
2/2✓ Branch 5 taken 5 times.
✓ Branch 6 taken 1 times.
|
6 | for (const auto & entry : bodiesList) |
511 | { | ||
512 |
1/2✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
|
5 | const MjcfBody & currentBody = mapOfBodies.at(entry); |
513 | |||
514 |
1/2✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
|
5 | addLinksToGeomModel(currentBody, *this, geomModel, meshLoader, type); |
515 | } | ||
516 | 1 | } | |
517 | } // namespace details | ||
518 | } // namespace mjcf | ||
519 | } // namespace pinocchio | ||
520 |