GCC Code Coverage Report

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