urdf.cpp
Go to the documentation of this file.
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 BOOST_AUTO_TEST_CASE ( build_model )
24 {
25  const std::string filename = PINOCCHIO_MODEL_DIR + std::string("/example-robot-data/robots/romeo_description/urdf/romeo_small.urdf");
26  const std::string dir = PINOCCHIO_MODEL_DIR;
27 
28  pinocchio::Model model;
29  pinocchio::urdf::buildModel(filename, model);
30  pinocchio::GeometryModel geomModel;
31  pinocchio::urdf::buildGeom(model, filename, pinocchio::COLLISION, geomModel, dir);
32 
33  BOOST_CHECK(model.nq == 31);
34 }
35 
36 BOOST_AUTO_TEST_CASE ( build_model_simple_humanoid )
37 {
38  const std::string filename = PINOCCHIO_MODEL_DIR + std::string("/simple_humanoid.urdf");
39  const std::string dir = PINOCCHIO_MODEL_DIR;
40 
41  pinocchio::Model model;
42  pinocchio::urdf::buildModel(filename, model);
43 
44  BOOST_CHECK_EQUAL(model.nq, 29);
45 
46  // Check that parsing collision_checking works.
47  pinocchio::GeometryModel geomModel;
48  pinocchio::urdf::buildGeom(model, filename, pinocchio::COLLISION, geomModel, dir);
49  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  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  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  pinocchio::Model model_ff;
74  pinocchio::urdf::buildModel(filename, pinocchio::JointModelFreeFlyer(), model_ff);
75 
76  BOOST_CHECK(model_ff.nq == 36);
77 }
78 
79 BOOST_AUTO_TEST_CASE ( build_model_from_XML )
80 {
81  const std::string filename = PINOCCHIO_MODEL_DIR + std::string("/example-robot-data/robots/romeo_description/urdf/romeo_small.urdf");
82 
83  // Read file as XML
84  std::ifstream file;
85  file.open(filename.c_str());
86  std::string filestr((std::istreambuf_iterator<char>(file)),
87  std::istreambuf_iterator<char>());
88 
89  pinocchio::Model model;
90  pinocchio::urdf::buildModelFromXML(filestr, model);
91 
92  BOOST_CHECK(model.nq == 31);
93 }
94 
95 BOOST_AUTO_TEST_CASE ( check_tree_from_XML )
96 {
97  // Read file as XML
98  std::string filestr(
99  "<?xml version=\"1.0\" encoding=\"utf-8\"?>"
100  "<robot name=\"test\">"
101  " <link name=\"base_link\"/>"
102  " <link name=\"link_1\"/>"
103  " <link name=\"link_2\"/>"
104  " <joint name=\"joint_1\" type=\"fixed\">"
105  " <origin xyz=\"1 0 0\"/>"
106  " <axis xyz=\"0 0 1\"/>"
107  " <parent link=\"base_link\"/>"
108  " <child link=\"link_1\"/>"
109  " </joint>"
110  " <joint name=\"joint_2\" type=\"fixed\">"
111  " <origin xyz=\"0 1 0\"/>"
112  " <axis xyz=\"0 0 1\"/>"
113  " <parent link=\"link_1\"/>"
114  " <child link=\"link_2\"/>"
115  " </joint>"
116  "</robot>"
117  );
118 
119  pinocchio::Model model;
120  pinocchio::urdf::buildModelFromXML(filestr, model);
121 
122  pinocchio::JointIndex
123  base_link_id = model.getFrameId("base_link"),
124  link1_id = model.getFrameId("link_1"),
125  link2_id = model.getFrameId("link_2"),
126  joint1_id = model.getFrameId("joint_1"),
127  joint2_id = model.getFrameId("joint_2");
128 
129  BOOST_CHECK_EQUAL(base_link_id, model.frames[joint1_id].previousFrame);
130  BOOST_CHECK_EQUAL(joint1_id , model.frames[link1_id ].previousFrame);
131  BOOST_CHECK_EQUAL(link1_id , model.frames[joint2_id].previousFrame);
132  BOOST_CHECK_EQUAL(joint2_id , model.frames[link2_id ].previousFrame);
133 }
134 
135 BOOST_AUTO_TEST_CASE ( build_model_from_UDRFTree )
136 {
137  const std::string filename = PINOCCHIO_MODEL_DIR + std::string("/example-robot-data/robots/romeo_description/urdf/romeo_small.urdf");
138 
139  ::urdf::ModelInterfaceSharedPtr urdfTree = ::urdf::parseURDFFile(filename);
140 
141  pinocchio::Model model;
142  pinocchio::urdf::buildModel(urdfTree, model);
143 
144  BOOST_CHECK(model.nq == 31);
145 }
146 
147 BOOST_AUTO_TEST_CASE ( build_model_with_joint )
148 {
149  const std::string filename = PINOCCHIO_MODEL_DIR + std::string("/example-robot-data/robots/romeo_description/urdf/romeo_small.urdf");
150  const std::string dir = PINOCCHIO_MODEL_DIR;
151 
152  pinocchio::Model model;
153  pinocchio::urdf::buildModel(filename, pinocchio::JointModelFreeFlyer(), model);
154  pinocchio::GeometryModel geomModel_collision, geomModel_visual;
155  pinocchio::urdf::buildGeom(model, filename, pinocchio::COLLISION, geomModel_collision, dir);
156  pinocchio::urdf::buildGeom(model, filename, pinocchio::VISUAL, geomModel_visual, dir);
157 
158  BOOST_CHECK(model.nq == 38);
159 }
160 
161 BOOST_AUTO_TEST_CASE ( build_model_with_joint_from_XML )
162 {
163  const std::string filename = PINOCCHIO_MODEL_DIR + std::string("/example-robot-data/robots/romeo_description/urdf/romeo_small.urdf");
164 
165  // Read file as XML
166  std::ifstream file;
167  file.open(filename.c_str());
168  std::string filestr((std::istreambuf_iterator<char>(file)),
169  std::istreambuf_iterator<char>());
170 
171  pinocchio::Model model;
172  pinocchio::urdf::buildModelFromXML(filestr, pinocchio::JointModelFreeFlyer(), model);
173 
174  BOOST_CHECK(model.nq == 38);
175 }
176 
177 BOOST_AUTO_TEST_CASE ( build_model_with_joint_from_UDRFTree )
178 {
179  const std::string filename = PINOCCHIO_MODEL_DIR + std::string("/example-robot-data/robots/romeo_description/urdf/romeo_small.urdf");
180 
181  ::urdf::ModelInterfaceSharedPtr urdfTree = ::urdf::parseURDFFile(filename);
182 
183  pinocchio::Model model;
184  pinocchio::urdf::buildModel(urdfTree, pinocchio::JointModelFreeFlyer(), model);
185 
186  BOOST_CHECK(model.nq == 38);
187 }
188 
189 BOOST_AUTO_TEST_CASE(append_two_URDF_models)
190 {
191  const std::string filename = PINOCCHIO_MODEL_DIR + std::string("/simple_humanoid.urdf");
192 
193  pinocchio::Model model;
194  pinocchio::urdf::buildModel(filename, model);
195 
196  BOOST_CHECK(model.njoints == 30);
197  const int nframes = model.nframes;
198  const std::string filestr(
199  "<?xml version=\"1.0\" encoding=\"utf-8\"?>"
200  "<robot name=\"test\">"
201  " <link name=\"box\"/>"
202  "</robot>"
203  );
204 
205  pinocchio::urdf::buildModelFromXML(filestr, model);
206  BOOST_CHECK(model.njoints == 30);
207  BOOST_CHECK(nframes + 1 == model.nframes);
208 }
209 
210 BOOST_AUTO_TEST_CASE(append_two_URDF_models_with_root_joint)
211 {
212  const std::string filename = PINOCCHIO_MODEL_DIR + std::string("/simple_humanoid.urdf");
213 
214  pinocchio::Model model;
215  pinocchio::urdf::buildModel(filename, pinocchio::JointModelFreeFlyer(), model);
216 
217  BOOST_CHECK(model.njoints == 31);
218  const std::string filestr(
219  "<?xml version=\"1.0\" encoding=\"utf-8\"?>"
220  "<robot name=\"test\">"
221  " <link name=\"box\"/>"
222  "</robot>"
223  );
224 
225 
226  BOOST_CHECK_THROW(pinocchio::urdf::buildModelFromXML(filestr, pinocchio::JointModelFreeFlyer(), model),
227  std::invalid_argument);
228 }
229 
230 BOOST_AUTO_TEST_CASE(check_specific_models)
231 {
232  const std::string filename = PINOCCHIO_MODEL_DIR + std::string("/baxter_simple.urdf");
233 
234  pinocchio::Model model;
235  pinocchio::urdf::buildModel(filename, model);
236 }
237 
238 BOOST_AUTO_TEST_SUITE_END()
pinocchio::ModelTpl::njoints
int njoints
Number of joints.
Definition: src/multibody/model.hpp:82
pinocchio::ModelTpl::frames
FrameVector frames
Vector of operational frames registered on the model.
Definition: src/multibody/model.hpp:145
PINOCCHIO_MODEL_DIR
#define PINOCCHIO_MODEL_DIR
Definition: build-reduced-model.cpp:11
pinocchio::ModelTpl::nframes
int nframes
Number of operational frames.
Definition: src/multibody/model.hpp:88
pinocchio::GeometryModel::geometryObjects
GeometryObjectVector geometryObjects
Vector of GeometryObjects used for collision computations.
Definition: src/multibody/geometry.hpp:171
pinocchio::ModelTpl::getFrameId
FrameIndex getFrameId(const std::string &name, const FrameType &type=(FrameType)(JOINT|FIXED_JOINT|BODY|OP_FRAME|SENSOR)) const
Returns the index of a frame given by its name.
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...
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: src/multibody/geometry.hpp:168
pinocchio::JointIndex
Index JointIndex
Definition: src/multibody/fwd.hpp:28
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...
BOOST_AUTO_TEST_CASE
BOOST_AUTO_TEST_CASE(build_model)
Definition: urdf.cpp:23
pinocchio::ModelTpl::nq
int nq
Dimension of the configuration vector representation.
Definition: src/multibody/model.hpp:76

pinocchio
Author(s):
autogenerated on Tue Jun 1 2021 02:45:05