urdf.cpp
Go to the documentation of this file.
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()
model.hpp
hpp::fcl::GEOM_CONVEX
GEOM_CONVEX
pinocchio::JointModelFreeFlyerTpl
Definition: multibody/joint/fwd.hpp:110
pinocchio::python::context::Model
ModelTpl< Scalar, Options > Model
Definition: bindings/python/context/generic.hpp:61
hpp::fcl::OT_BVH
OT_BVH
pinocchio::VISUAL
@ VISUAL
Definition: multibody/geometry-object.hpp:26
anymal-simulation.model
model
Definition: anymal-simulation.py:12
pinocchio::GeometryModel::geometryObjects
GeometryObjectVector geometryObjects
Vector of GeometryObjects used for collision computations.
Definition: multibody/geometry.hpp:217
pinocchio::urdf::buildModel
ModelTpl< Scalar, Options, JointCollectionTpl > & buildModel(const std::string &filename, const typename ModelTpl< Scalar, Options, JointCollectionTpl >::JointModel &rootJoint, ModelTpl< Scalar, Options, JointCollectionTpl > &model, const bool verbose=false)
Build the model from a URDF file with a particular joint as root of the model tree inside the model g...
filename
filename
urdf.hpp
pinocchio::urdf::buildGeom
GeometryModel & buildGeom(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const std::string &filename, const GeometryType type, GeometryModel &geom_model, const std::vector< std::string > &package_paths=std::vector< std::string >(), ::hpp::fcl::MeshLoaderPtr mesh_loader=::hpp::fcl::MeshLoaderPtr())
Build The GeometryModel from a URDF file. Search for meshes in the directories specified by the user ...
pinocchio::GeometryModel::ngeoms
Index ngeoms
The number of GeometryObjects.
Definition: multibody/geometry.hpp:214
pinocchio::urdf::buildModelFromXML
ModelTpl< Scalar, Options, JointCollectionTpl > & buildModelFromXML(const std::string &xml_stream, const typename ModelTpl< Scalar, Options, JointCollectionTpl >::JointModel &rootJoint, ModelTpl< Scalar, Options, JointCollectionTpl > &model, const bool verbose=false)
Build the model from an XML stream with a particular joint as root of the model tree inside the model...
simulation-closed-kinematic-chains.joint1_id
joint1_id
Definition: simulation-closed-kinematic-chains.py:56
simulation-closed-kinematic-chains.joint2_id
joint2_id
Definition: simulation-closed-kinematic-chains.py:66
append-urdf-model-with-another-model.model1
model1
Definition: append-urdf-model-with-another-model.py:19
hpp::fcl::GEOM_CYLINDER
GEOM_CYLINDER
pinocchio::FIXED_JOINT
@ FIXED_JOINT
fixed joint frame: joint frame but for a fixed joint
Definition: multibody/frame.hpp:35
pinocchio::JointIndex
Index JointIndex
Definition: multibody/fwd.hpp:26
pinocchio::GeometryModel
Definition: multibody/geometry.hpp:50
pinocchio::COLLISION
@ COLLISION
Definition: multibody/geometry-object.hpp:27
BOOST_AUTO_TEST_CASE
BOOST_AUTO_TEST_CASE(build_model)
Definition: urdf.cpp:22
pinocchio::ModelTpl
Definition: context/generic.hpp:20
pinocchio::BODY
@ BODY
body frame: attached to the collision, inertial or visual properties of a link
Definition: multibody/frame.hpp:36
collision_object.h
hpp::fcl::GEOM_CAPSULE
GEOM_CAPSULE
pinocchio::ModelTpl::nq
int nq
Dimension of the configuration vector representation.
Definition: multibody/model.hpp:99
BOOST_CHECK
#define BOOST_CHECK(check)
Definition: overview-urdf.cpp:34
PINOCCHIO_MODEL_DIR
#define PINOCCHIO_MODEL_DIR
Definition: build-reduced-model.cpp:11

pinocchio
Author(s):
autogenerated on Sat Jun 22 2024 02:41:50