GCC Code Coverage Report

Directory: ./
File: unittest/urdf.cpp
Date: 2024-08-27 18:20:05
Exec Total Coverage
Lines: 0 127 0.0%
Branches: 0 1526 0.0%
Line Branch Exec Source
1 //
2 // Copyright (c) 2015-2022 CNRS INRIA
3 //
4
5 #include <iostream>
6 #include <fstream>
7 #include <streambuf>
8
9 #include "pinocchio/multibody/model.hpp"
10 #include "pinocchio/parsers/urdf.hpp"
11
12 #ifdef PINOCCHIO_WITH_HPP_FCL
13 #include <hpp/fcl/collision_object.h>
14 #endif // PINOCCHIO_WITH_HPP_FCL
15
16 #include <boost/test/unit_test.hpp>
17
18 #include <urdf_parser/urdf_parser.h>
19
20 BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)
21
22 ✗ BOOST_AUTO_TEST_CASE(build_model)
23 {
24 const std::string filename =
25 PINOCCHIO_MODEL_DIR
26 ✗ + std::string("/example-robot-data/robots/romeo_description/urdf/romeo_small.urdf");
27 ✗ const std::string dir = PINOCCHIO_MODEL_DIR;
28
29 ✗ pinocchio::Model model;
30 ✗ pinocchio::urdf::buildModel(filename, model);
31 ✗ pinocchio::GeometryModel geomModel;
32 ✗ pinocchio::urdf::buildGeom(model, filename, pinocchio::COLLISION, geomModel, dir);
33
34 ✗ BOOST_CHECK(model.nq == 31);
35 }
36
37 ✗ BOOST_AUTO_TEST_CASE(build_model_simple_humanoid)
38 {
39 ✗ const std::string filename = PINOCCHIO_MODEL_DIR + std::string("/simple_humanoid.urdf");
40 ✗ const std::string dir = PINOCCHIO_MODEL_DIR;
41
42 ✗ pinocchio::Model model;
43 ✗ pinocchio::urdf::buildModel(filename, model);
44
45 ✗ BOOST_CHECK_EQUAL(model.nq, 29);
46
47 // Check that parsing collision_checking works.
48 ✗ pinocchio::GeometryModel geomModel;
49 ✗ pinocchio::urdf::buildGeom(model, filename, pinocchio::COLLISION, geomModel, dir);
50 ✗ BOOST_CHECK_EQUAL(geomModel.ngeoms, 2);
51
52 #ifdef PINOCCHIO_WITH_HPP_FCL
53 // Check that cylinder is converted into capsule.
54 #ifdef PINOCCHIO_URDFDOM_COLLISION_WITH_GROUP_NAME
55 BOOST_CHECK_EQUAL(geomModel.geometryObjects[0].geometry->getNodeType(), hpp::fcl::GEOM_CYLINDER);
56 #else // PINOCCHIO_URDFDOM_COLLISION_WITH_GROUP_NAME
57 ✗ BOOST_CHECK_EQUAL(geomModel.geometryObjects[0].geometry->getNodeType(), hpp::fcl::GEOM_CAPSULE);
58 #endif
59
60 #ifndef PINOCCHIO_URDFDOM_COLLISION_WITH_GROUP_NAME
61 ✗ BOOST_CHECK_EQUAL(geomModel.geometryObjects[1].geometry->getNodeType(), hpp::fcl::GEOM_CONVEX);
62 #else // PINOCCHIO_URDFDOM_COLLISION_WITH_GROUP_NAME
63 BOOST_CHECK_EQUAL(geomModel.geometryObjects[1].geometry->getObjectType(), hpp::fcl::OT_BVH);
64 #endif
65 #endif // PINOCCHIO_WITH_HPP_FCL
66
67 ✗ pinocchio::Model model_ff;
68 ✗ pinocchio::urdf::buildModel(filename, pinocchio::JointModelFreeFlyer(), model_ff);
69
70 ✗ BOOST_CHECK(model_ff.nq == 36);
71 }
72
73 ✗ BOOST_AUTO_TEST_CASE(check_mesh_relative_path)
74 {
75 ✗ std::string filename = PINOCCHIO_MODEL_DIR + std::string("/simple_humanoid.urdf");
76 ✗ const std::string dir = PINOCCHIO_MODEL_DIR;
77
78 ✗ pinocchio::Model model0;
79 ✗ pinocchio::urdf::buildModel(filename, model0);
80 ✗ pinocchio::GeometryModel geomModel0;
81 ✗ pinocchio::urdf::buildGeom(model0, filename, pinocchio::COLLISION, geomModel0, dir);
82 ✗ BOOST_CHECK_EQUAL(geomModel0.ngeoms, 2);
83
84 // check if urdf with relative mesh path without //package can be loaded
85 ✗ filename = PINOCCHIO_MODEL_DIR + std::string("/simple_humanoid_rel_mesh.urdf");
86 ✗ pinocchio::Model model1;
87 ✗ pinocchio::urdf::buildModel(filename, model1);
88 ✗ pinocchio::GeometryModel geomModel1;
89 ✗ pinocchio::urdf::buildGeom(model1, filename, pinocchio::COLLISION, geomModel1, dir);
90 ✗ BOOST_CHECK_EQUAL(geomModel1.ngeoms, 2);
91
92 ✗ BOOST_CHECK_EQUAL(
93 geomModel0.geometryObjects[1].name.compare(geomModel1.geometryObjects[1].name), 0);
94 }
95
96 ✗ BOOST_AUTO_TEST_CASE(build_model_from_XML)
97 {
98 const std::string filename =
99 PINOCCHIO_MODEL_DIR
100 ✗ + std::string("/example-robot-data/robots/romeo_description/urdf/romeo_small.urdf");
101
102 // Read file as XML
103 ✗ std::ifstream file;
104 ✗ file.open(filename.c_str());
105 ✗ std::string filestr((std::istreambuf_iterator<char>(file)), std::istreambuf_iterator<char>());
106
107 ✗ pinocchio::Model model;
108 ✗ pinocchio::urdf::buildModelFromXML(filestr, model);
109
110 ✗ BOOST_CHECK(model.nq == 31);
111 }
112
113 ✗ BOOST_AUTO_TEST_CASE(check_tree_from_XML)
114 {
115 // Read file as XML
116 std::string filestr("<?xml version=\"1.0\" encoding=\"utf-8\"?>"
117 "<robot name=\"test\">"
118 " <link name=\"base_link\"/>"
119 " <link name=\"link_1\"/>"
120 " <link name=\"link_2\"/>"
121 " <joint name=\"joint_1\" type=\"fixed\">"
122 " <origin xyz=\"1 0 0\"/>"
123 " <axis xyz=\"0 0 1\"/>"
124 " <parent link=\"base_link\"/>"
125 " <child link=\"link_1\"/>"
126 " </joint>"
127 " <joint name=\"joint_2\" type=\"fixed\">"
128 " <origin xyz=\"0 1 0\"/>"
129 " <axis xyz=\"0 0 1\"/>"
130 " <parent link=\"link_1\"/>"
131 " <child link=\"link_2\"/>"
132 " </joint>"
133 ✗ "</robot>");
134
135 ✗ pinocchio::Model model;
136 ✗ pinocchio::urdf::buildModelFromXML(filestr, model);
137
138 ✗ pinocchio::JointIndex base_link_id = model.getFrameId("base_link"),
139 ✗ link1_id = model.getFrameId("link_1"),
140 ✗ link2_id = model.getFrameId("link_2"),
141 ✗ joint1_id = model.getFrameId("joint_1"),
142 ✗ joint2_id = model.getFrameId("joint_2");
143
144 ✗ BOOST_CHECK_EQUAL(base_link_id, model.frames[joint1_id].parentFrame);
145 ✗ BOOST_CHECK_EQUAL(joint1_id, model.frames[link1_id].parentFrame);
146 ✗ BOOST_CHECK_EQUAL(link1_id, model.frames[joint2_id].parentFrame);
147 ✗ BOOST_CHECK_EQUAL(joint2_id, model.frames[link2_id].parentFrame);
148 }
149
150 ✗ BOOST_AUTO_TEST_CASE(build_model_from_UDRFTree)
151 {
152 const std::string filename =
153 PINOCCHIO_MODEL_DIR
154 ✗ + std::string("/example-robot-data/robots/romeo_description/urdf/romeo_small.urdf");
155
156 ✗ ::urdf::ModelInterfaceSharedPtr urdfTree = ::urdf::parseURDFFile(filename);
157
158 ✗ pinocchio::Model model;
159 ✗ pinocchio::urdf::buildModel(urdfTree, model);
160
161 ✗ BOOST_CHECK(model.nq == 31);
162 }
163
164 ✗ BOOST_AUTO_TEST_CASE(build_model_with_joint)
165 {
166 const std::string filename =
167 PINOCCHIO_MODEL_DIR
168 ✗ + std::string("/example-robot-data/robots/romeo_description/urdf/romeo_small.urdf");
169 ✗ const std::string dir = PINOCCHIO_MODEL_DIR;
170
171 ✗ pinocchio::Model model;
172 ✗ pinocchio::urdf::buildModel(filename, pinocchio::JointModelFreeFlyer(), model);
173 ✗ pinocchio::GeometryModel geomModel_collision, geomModel_visual;
174 ✗ pinocchio::urdf::buildGeom(model, filename, pinocchio::COLLISION, geomModel_collision, dir);
175 ✗ pinocchio::urdf::buildGeom(model, filename, pinocchio::VISUAL, geomModel_visual, dir);
176
177 ✗ BOOST_CHECK(model.nq == 38);
178 }
179
180 ✗ BOOST_AUTO_TEST_CASE(build_model_with_joint_from_XML)
181 {
182 const std::string filename =
183 PINOCCHIO_MODEL_DIR
184 ✗ + std::string("/example-robot-data/robots/romeo_description/urdf/romeo_small.urdf");
185
186 // Read file as XML
187 ✗ std::ifstream file;
188 ✗ file.open(filename.c_str());
189 ✗ std::string filestr((std::istreambuf_iterator<char>(file)), std::istreambuf_iterator<char>());
190
191 ✗ pinocchio::Model model;
192 ✗ pinocchio::urdf::buildModelFromXML(filestr, pinocchio::JointModelFreeFlyer(), model);
193
194 ✗ BOOST_CHECK(model.nq == 38);
195 }
196
197 ✗ BOOST_AUTO_TEST_CASE(build_model_with_joint_from_UDRFTree)
198 {
199 const std::string filename =
200 PINOCCHIO_MODEL_DIR
201 ✗ + std::string("/example-robot-data/robots/romeo_description/urdf/romeo_small.urdf");
202
203 ✗ ::urdf::ModelInterfaceSharedPtr urdfTree = ::urdf::parseURDFFile(filename);
204
205 ✗ pinocchio::Model model;
206 ✗ pinocchio::urdf::buildModel(urdfTree, pinocchio::JointModelFreeFlyer(), model);
207
208 ✗ BOOST_CHECK(model.nq == 38);
209 }
210
211 ✗ BOOST_AUTO_TEST_CASE(append_two_URDF_models)
212 {
213 ✗ const std::string filename = PINOCCHIO_MODEL_DIR + std::string("/simple_humanoid.urdf");
214
215 ✗ pinocchio::Model model;
216 ✗ pinocchio::urdf::buildModel(filename, model);
217
218 ✗ BOOST_CHECK(model.njoints == 30);
219 ✗ const int nframes = model.nframes;
220 const std::string filestr("<?xml version=\"1.0\" encoding=\"utf-8\"?>"
221 "<robot name=\"test\">"
222 " <link name=\"box\"/>"
223 ✗ "</robot>");
224
225 ✗ pinocchio::urdf::buildModelFromXML(filestr, model);
226 ✗ BOOST_CHECK(model.njoints == 30);
227 ✗ BOOST_CHECK(nframes + 1 == model.nframes);
228 }
229
230 ✗ BOOST_AUTO_TEST_CASE(append_two_URDF_models_with_root_joint)
231 {
232 ✗ const std::string filename = PINOCCHIO_MODEL_DIR + std::string("/simple_humanoid.urdf");
233
234 ✗ pinocchio::Model model;
235 ✗ pinocchio::urdf::buildModel(filename, pinocchio::JointModelFreeFlyer(), model);
236
237 ✗ BOOST_CHECK(model.njoints == 31);
238 const std::string filestr("<?xml version=\"1.0\" encoding=\"utf-8\"?>"
239 "<robot name=\"test\">"
240 " <link name=\"box\"/>"
241 ✗ "</robot>");
242
243 ✗ BOOST_CHECK_THROW(
244 pinocchio::urdf::buildModelFromXML(filestr, pinocchio::JointModelFreeFlyer(), model),
245 std::invalid_argument);
246 }
247
248 ✗ BOOST_AUTO_TEST_CASE(check_specific_models)
249 {
250 ✗ const std::string filename = PINOCCHIO_MODEL_DIR + std::string("/baxter_simple.urdf");
251
252 ✗ pinocchio::Model model;
253 ✗ pinocchio::urdf::buildModel(filename, model);
254 }
255
256 #if defined(PINOCCHIO_WITH_HPP_FCL)
257 ✗ BOOST_AUTO_TEST_CASE(test_geometry_parsing)
258 {
259 typedef pinocchio::Model Model;
260 typedef pinocchio::GeometryModel GeometryModel;
261
262 std::string filename =
263 PINOCCHIO_MODEL_DIR
264 ✗ + std::string("/example-robot-data/robots/romeo_description/urdf/romeo_small.urdf");
265 ✗ std::vector<std::string> packageDirs;
266 ✗ std::string meshDir = PINOCCHIO_MODEL_DIR;
267 ✗ packageDirs.push_back(meshDir);
268
269 ✗ Model model;
270 ✗ pinocchio::urdf::buildModel(filename, pinocchio::JointModelFreeFlyer(), model);
271 ✗ GeometryModel geomModel;
272 ✗ pinocchio::urdf::buildGeom(model, filename, pinocchio::COLLISION, geomModel, packageDirs);
273 ✗ geomModel.addAllCollisionPairs();
274
275 GeometryModel geomModelOther =
276 ✗ pinocchio::urdf::buildGeom(model, filename, pinocchio::COLLISION, geomModel, packageDirs);
277 ✗ BOOST_CHECK(geomModelOther == geomModel);
278 }
279 #endif // if defined(PINOCCHIO_WITH_HPP_FCL)
280
281 ✗ BOOST_AUTO_TEST_CASE(test_getFrameId_identical_link_and_joint_name)
282 {
283 // This test checks whether the input argument of getFrameId raises an exception when multiple
284 // frames with identical names are found. Note, a model that contains a link and a joint with the
285 // same name is valid, but the look-up for e.g. getFrameId requires the explicit specification of
286 // the FrameType in order to be valid. It resolved
287 // https://github.com/stack-of-tasks/pinocchio/issues/1771
288 ✗ pinocchio::Model model;
289 const std::string filename =
290 ✗ PINOCCHIO_MODEL_DIR + std::string("/../unittest/models/link_and_joint_identical_name.urdf");
291 ✗ pinocchio::urdf::buildModel(filename, model);
292
293 ✗ BOOST_CHECK_THROW(model.getFrameId("base"), std::invalid_argument);
294 ✗ BOOST_CHECK(model.getFrameId("base", pinocchio::FrameType::BODY) == 2);
295 ✗ BOOST_CHECK(model.getFrameId("base", pinocchio::FrameType::FIXED_JOINT) == 3);
296 }
297
298 BOOST_AUTO_TEST_SUITE_END()
299