GCC Code Coverage Report

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