GCC Code Coverage Report
Directory: ./ Exec Total Coverage
File: unittest/urdf.cpp Lines: 128 128 100.0 %
Date: 2023-08-09 08:43:58 Branches: 696 1416 49.2 %

Line Branch Exec Source
1
//
2
// Copyright (c) 2015-2021 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
21
BOOST_AUTO_TEST_SUITE ( BOOST_TEST_MODULE )
22
23
















4
BOOST_AUTO_TEST_CASE ( build_model )
24
{
25

6
  const std::string filename = PINOCCHIO_MODEL_DIR + std::string("/example-robot-data/robots/romeo_description/urdf/romeo_small.urdf");
26
4
  const std::string dir = PINOCCHIO_MODEL_DIR;
27
28
4
  pinocchio::Model model;
29
2
  pinocchio::urdf::buildModel(filename, model);
30
4
  pinocchio::GeometryModel geomModel;
31
2
  pinocchio::urdf::buildGeom(model, filename, pinocchio::COLLISION, geomModel, dir);
32
33



2
  BOOST_CHECK(model.nq == 31);
34
2
}
35
36
















4
BOOST_AUTO_TEST_CASE ( build_model_simple_humanoid )
37
{
38

6
  const std::string filename = PINOCCHIO_MODEL_DIR + std::string("/simple_humanoid.urdf");
39
4
  const std::string dir = PINOCCHIO_MODEL_DIR;
40
41
4
  pinocchio::Model model;
42
2
  pinocchio::urdf::buildModel(filename, model);
43
44


2
  BOOST_CHECK_EQUAL(model.nq, 29);
45
46
  // Check that parsing collision_checking works.
47
4
  pinocchio::GeometryModel geomModel;
48
2
  pinocchio::urdf::buildGeom(model, filename, pinocchio::COLLISION, geomModel, dir);
49


2
  BOOST_CHECK_EQUAL(geomModel.ngeoms, 2);
50
51
#ifdef PINOCCHIO_WITH_HPP_FCL
52
  // Check that cylinder is converted into capsule.
53
#ifdef PINOCCHIO_URDFDOM_COLLISION_WITH_GROUP_NAME
54
  BOOST_CHECK_EQUAL(geomModel.geometryObjects[0].geometry->getNodeType(), hpp::fcl::GEOM_CYLINDER);
55
#else // PINOCCHIO_URDFDOM_COLLISION_WITH_GROUP_NAME
56



2
  BOOST_CHECK_EQUAL(geomModel.geometryObjects[0].geometry->getNodeType(), hpp::fcl::GEOM_CAPSULE);
57
#endif
58
59
#if ( HPP_FCL_MAJOR_VERSION>1 || ( HPP_FCL_MAJOR_VERSION==1 && \
60
      ( HPP_FCL_MINOR_VERSION>1 || ( HPP_FCL_MINOR_VERSION==1 && \
61
                                     HPP_FCL_PATCH_VERSION>3))))
62
#  define PINOCCHIO_HPP_FCL_SUPERIOR_TO_1_1_3
63
#endif
64
65
#if defined(PINOCCHIO_HPP_FCL_SUPERIOR_TO_1_1_3) && !defined(PINOCCHIO_URDFDOM_COLLISION_WITH_GROUP_NAME)
66



2
  BOOST_CHECK_EQUAL(geomModel.geometryObjects[1].geometry->getNodeType(), hpp::fcl::GEOM_CONVEX);
67
#undef PINOCCHIO_HPP_FCL_SUPERIOR_TO_1_1_3
68
#else // PINOCCHIO_HPP_FCL_SUPERIOR_TO_1_1_3 && !PINOCCHIO_URDFDOM_COLLISION_WITH_GROUP_NAME
69
  BOOST_CHECK_EQUAL(geomModel.geometryObjects[1].geometry->getObjectType(), hpp::fcl::OT_BVH);
70
#endif // PINOCCHIO_HPP_FCL_SUPERIOR_TO_1_1_3 && !PINOCCHIO_URDFDOM_COLLISION_WITH_GROUP_NAME
71
#endif // PINOCCHIO_WITH_HPP_FCL
72
73
4
  pinocchio::Model model_ff;
74

2
  pinocchio::urdf::buildModel(filename, pinocchio::JointModelFreeFlyer(), model_ff);
75
76



2
  BOOST_CHECK(model_ff.nq == 36);
77
2
}
78
79
















4
BOOST_AUTO_TEST_CASE ( check_mesh_relative_path )
80
{
81

6
  std::string filename = PINOCCHIO_MODEL_DIR + std::string("/simple_humanoid.urdf");
82
4
  const std::string dir = PINOCCHIO_MODEL_DIR;
83
84
4
  pinocchio::Model model0;
85
2
  pinocchio::urdf::buildModel(filename, model0);
86
4
  pinocchio::GeometryModel geomModel0;
87
2
  pinocchio::urdf::buildGeom(model0, filename, pinocchio::COLLISION, geomModel0, dir);
88


2
  BOOST_CHECK_EQUAL(geomModel0.ngeoms, 2);
89
90
  // check if urdf with relative mesh path without //package can be loaded
91

2
  filename = PINOCCHIO_MODEL_DIR + std::string("/simple_humanoid_rel_mesh.urdf");
92
4
  pinocchio::Model model1;
93
2
  pinocchio::urdf::buildModel(filename, model1);
94
4
  pinocchio::GeometryModel geomModel1;
95
2
  pinocchio::urdf::buildGeom(model1, filename, pinocchio::COLLISION, geomModel1, dir);
96


2
  BOOST_CHECK_EQUAL(geomModel1.ngeoms, 2);
97
98



2
  BOOST_CHECK_EQUAL(geomModel0.geometryObjects[1].name.compare(geomModel1.geometryObjects[1].name), 0);
99
2
}
100
101
















4
BOOST_AUTO_TEST_CASE ( build_model_from_XML )
102
{
103

6
  const std::string filename = PINOCCHIO_MODEL_DIR + std::string("/example-robot-data/robots/romeo_description/urdf/romeo_small.urdf");
104
105
  // Read file as XML
106
4
  std::ifstream file;
107
2
  file.open(filename.c_str());
108
  std::string filestr((std::istreambuf_iterator<char>(file)),
109
4
                      std::istreambuf_iterator<char>());
110
111
4
  pinocchio::Model model;
112
2
  pinocchio::urdf::buildModelFromXML(filestr, model);
113
114



2
  BOOST_CHECK(model.nq == 31);
115
2
}
116
117
















4
BOOST_AUTO_TEST_CASE ( check_tree_from_XML )
118
{
119
  // Read file as XML
120
  std::string filestr(
121
      "<?xml version=\"1.0\" encoding=\"utf-8\"?>"
122
      "<robot name=\"test\">"
123
      "  <link name=\"base_link\"/>"
124
      "  <link name=\"link_1\"/>"
125
      "  <link name=\"link_2\"/>"
126
      "  <joint name=\"joint_1\" type=\"fixed\">"
127
      "    <origin xyz=\"1 0 0\"/>"
128
      "    <axis xyz=\"0 0 1\"/>"
129
      "    <parent link=\"base_link\"/>"
130
      "    <child link=\"link_1\"/>"
131
      "  </joint>"
132
      "  <joint name=\"joint_2\" type=\"fixed\">"
133
      "    <origin xyz=\"0 1 0\"/>"
134
      "    <axis xyz=\"0 0 1\"/>"
135
      "    <parent link=\"link_1\"/>"
136
      "    <child link=\"link_2\"/>"
137
      "  </joint>"
138
      "</robot>"
139
4
      );
140
141
4
  pinocchio::Model model;
142
2
  pinocchio::urdf::buildModelFromXML(filestr, model);
143
144
  pinocchio::JointIndex
145

2
    base_link_id = model.getFrameId("base_link"),
146

2
    link1_id     = model.getFrameId("link_1"),
147

2
    link2_id     = model.getFrameId("link_2"),
148

2
    joint1_id    = model.getFrameId("joint_1"),
149

2
    joint2_id    = model.getFrameId("joint_2");
150
151


2
  BOOST_CHECK_EQUAL(base_link_id, model.frames[joint1_id].previousFrame);
152


2
  BOOST_CHECK_EQUAL(joint1_id   , model.frames[link1_id ].previousFrame);
153


2
  BOOST_CHECK_EQUAL(link1_id    , model.frames[joint2_id].previousFrame);
154


2
  BOOST_CHECK_EQUAL(joint2_id   , model.frames[link2_id ].previousFrame);
155
2
}
156
157
















4
BOOST_AUTO_TEST_CASE ( build_model_from_UDRFTree )
158
{
159

6
  const std::string filename = PINOCCHIO_MODEL_DIR + std::string("/example-robot-data/robots/romeo_description/urdf/romeo_small.urdf");
160
161
4
  ::urdf::ModelInterfaceSharedPtr urdfTree = ::urdf::parseURDFFile(filename);
162
163
4
  pinocchio::Model model;
164
2
  pinocchio::urdf::buildModel(urdfTree, model);
165
166



2
  BOOST_CHECK(model.nq == 31);
167
2
}
168
169
















4
BOOST_AUTO_TEST_CASE ( build_model_with_joint )
170
{
171

6
  const std::string filename = PINOCCHIO_MODEL_DIR + std::string("/example-robot-data/robots/romeo_description/urdf/romeo_small.urdf");
172
4
  const std::string dir = PINOCCHIO_MODEL_DIR;
173
174
4
  pinocchio::Model model;
175

2
  pinocchio::urdf::buildModel(filename, pinocchio::JointModelFreeFlyer(), model);
176

4
  pinocchio::GeometryModel geomModel_collision, geomModel_visual;
177
2
  pinocchio::urdf::buildGeom(model, filename, pinocchio::COLLISION, geomModel_collision, dir);
178
2
  pinocchio::urdf::buildGeom(model, filename, pinocchio::VISUAL, geomModel_visual, dir);
179
180



2
  BOOST_CHECK(model.nq == 38);
181
2
}
182
183
















4
BOOST_AUTO_TEST_CASE ( build_model_with_joint_from_XML )
184
{
185

6
  const std::string filename = PINOCCHIO_MODEL_DIR + std::string("/example-robot-data/robots/romeo_description/urdf/romeo_small.urdf");
186
187
  // Read file as XML
188
4
  std::ifstream file;
189
2
  file.open(filename.c_str());
190
  std::string filestr((std::istreambuf_iterator<char>(file)),
191
4
                      std::istreambuf_iterator<char>());
192
193
4
  pinocchio::Model model;
194

2
  pinocchio::urdf::buildModelFromXML(filestr, pinocchio::JointModelFreeFlyer(), model);
195
196



2
  BOOST_CHECK(model.nq == 38);
197
2
}
198
199
















4
BOOST_AUTO_TEST_CASE ( build_model_with_joint_from_UDRFTree )
200
{
201

6
  const std::string filename = PINOCCHIO_MODEL_DIR + std::string("/example-robot-data/robots/romeo_description/urdf/romeo_small.urdf");
202
203
4
  ::urdf::ModelInterfaceSharedPtr urdfTree = ::urdf::parseURDFFile(filename);
204
205
4
  pinocchio::Model model;
206

2
  pinocchio::urdf::buildModel(urdfTree, pinocchio::JointModelFreeFlyer(), model);
207
208



2
  BOOST_CHECK(model.nq == 38);
209
2
}
210
211
















4
BOOST_AUTO_TEST_CASE(append_two_URDF_models)
212
{
213

6
  const std::string filename = PINOCCHIO_MODEL_DIR + std::string("/simple_humanoid.urdf");
214
215
4
  pinocchio::Model model;
216
2
  pinocchio::urdf::buildModel(filename, model);
217
218



2
  BOOST_CHECK(model.njoints == 30);
219
2
  const int nframes = model.nframes;
220
  const std::string filestr(
221
                            "<?xml version=\"1.0\" encoding=\"utf-8\"?>"
222
                            "<robot name=\"test\">"
223
                            "  <link name=\"box\"/>"
224
                            "</robot>"
225
4
                            );
226
227
2
  pinocchio::urdf::buildModelFromXML(filestr, model);
228



2
  BOOST_CHECK(model.njoints == 30);
229



2
  BOOST_CHECK(nframes + 1 == model.nframes);
230
2
}
231
232
















4
BOOST_AUTO_TEST_CASE(append_two_URDF_models_with_root_joint)
233
{
234

6
  const std::string filename = PINOCCHIO_MODEL_DIR + std::string("/simple_humanoid.urdf");
235
236
4
  pinocchio::Model model;
237

2
  pinocchio::urdf::buildModel(filename, pinocchio::JointModelFreeFlyer(), model);
238
239



2
  BOOST_CHECK(model.njoints == 31);
240
  const std::string filestr(
241
                            "<?xml version=\"1.0\" encoding=\"utf-8\"?>"
242
                            "<robot name=\"test\">"
243
                            "  <link name=\"box\"/>"
244
                            "</robot>"
245
6
                            );
246
247
248









8
  BOOST_CHECK_THROW(pinocchio::urdf::buildModelFromXML(filestr, pinocchio::JointModelFreeFlyer(), model),
249
                    std::invalid_argument);
250
2
}
251
252
















4
BOOST_AUTO_TEST_CASE(check_specific_models)
253
{
254

6
  const std::string filename = PINOCCHIO_MODEL_DIR + std::string("/baxter_simple.urdf");
255
256
4
  pinocchio::Model model;
257
2
  pinocchio::urdf::buildModel(filename, model);
258
2
}
259
260
















4
BOOST_AUTO_TEST_CASE(test_getFrameId_identical_link_and_joint_name)
261
{
262
    // This test checks whether the input argument of getFrameId raises an exception when multiple frames with identical names are found.
263
    // Note, a model that contains a link and a joint with the same name is valid, but the look-up for e.g. getFrameId requires the explicit
264
    // specification of the FrameType in order to be valid.
265
    // It resolved https://github.com/stack-of-tasks/pinocchio/issues/1771
266
4
    pinocchio::Model model;
267

6
    const std::string filename = PINOCCHIO_MODEL_DIR + std::string("/../unittest/models/link_and_joint_identical_name.urdf");
268
2
    pinocchio::urdf::buildModel(filename,model);
269
270








10
    BOOST_CHECK_THROW(model.getFrameId("base"), std::invalid_argument);
271




2
    BOOST_CHECK(model.getFrameId("base", pinocchio::FrameType::BODY) == 2);
272




2
    BOOST_CHECK(model.getFrameId("base", pinocchio::FrameType::FIXED_JOINT) == 3);
273
2
}
274
275
BOOST_AUTO_TEST_SUITE_END()