tesseract_urdf_inertial_unit.cpp
Go to the documentation of this file.
1 #include <tesseract_common/macros.h>
2 TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
3 #include <gtest/gtest.h>
4 #include <Eigen/Geometry>
5 TESSERACT_COMMON_IGNORE_WARNINGS_POP
6 
7 #include <tesseract_urdf/inertial.h>
8 #include "tesseract_urdf_common_unit.h"
9 
10 TEST(TesseractURDFUnit, parse_inertial) // NOLINT
11 {
12  {
13  std::string str = R"(<inertial extra="0 0 0">
14  <origin xyz="0 0 0" rpy="0 0 0"/>
15  <mass value="1.0" extra="0 0 0"/>
16  <inertia ixx="1.0" ixy="2.0" ixz="3.0" iyy="4.0" iyz="5.0" izz="6.0" extra="0 0 0"/>
17  </inertial>)";
18  tesseract_scene_graph::Inertial::Ptr elem;
19  EXPECT_TRUE(runTest<tesseract_scene_graph::Inertial::Ptr>(
20  elem, &tesseract_urdf::parseInertial, str, tesseract_urdf::INERTIAL_ELEMENT_NAME.data()));
21  EXPECT_NEAR(elem->mass, 1, 1e-8);
22  EXPECT_NEAR(elem->ixx, 1, 1e-8);
23  EXPECT_NEAR(elem->ixy, 2, 1e-8);
24  EXPECT_NEAR(elem->ixz, 3, 1e-8);
25  EXPECT_NEAR(elem->iyy, 4, 1e-8);
26  EXPECT_NEAR(elem->iyz, 5, 1e-8);
27  EXPECT_NEAR(elem->izz, 6, 1e-8);
28  }
29 
30  {
31  std::string str = R"(<inertial>
32  <mass value="1.0"/>
33  <inertia ixx="1.0" ixy="2.0" ixz="3.0" iyy="4.0" iyz="5.0" izz="6.0"/>
34  </inertial>)";
35  tesseract_scene_graph::Inertial::Ptr elem;
36  EXPECT_TRUE(runTest<tesseract_scene_graph::Inertial::Ptr>(
37  elem, &tesseract_urdf::parseInertial, str, tesseract_urdf::INERTIAL_ELEMENT_NAME.data()));
38  EXPECT_NEAR(elem->mass, 1, 1e-8);
39  EXPECT_NEAR(elem->ixx, 1, 1e-8);
40  EXPECT_NEAR(elem->ixy, 2, 1e-8);
41  EXPECT_NEAR(elem->ixz, 3, 1e-8);
42  EXPECT_NEAR(elem->iyy, 4, 1e-8);
43  EXPECT_NEAR(elem->iyz, 5, 1e-8);
44  EXPECT_NEAR(elem->izz, 6, 1e-8);
45  }
46 
47  {
48  std::string str = R"(<inertial extra="0 0 0">
49  <origin xyz="0 0 0 4" rpy="0 0 0"/>
50  <mass value="1.0" extra="0 0 0"/>
51  <inertia ixx="1.0" ixy="2.0" ixz="3.0" iyy="4.0" iyz="5.0" izz="6.0" extra="0 0 0"/>
52  </inertial>)";
53  tesseract_scene_graph::Inertial::Ptr elem;
54  EXPECT_FALSE(runTest<tesseract_scene_graph::Inertial::Ptr>(
55  elem, &tesseract_urdf::parseInertial, str, tesseract_urdf::INERTIAL_ELEMENT_NAME.data()));
56  }
57 
58  {
59  std::string str = R"(<inertial>
60  <inertia ixx="1.0" ixy="2.0" ixz="3.0" iyy="4.0" iyz="5.0" izz="6.0"/>
61  </inertial>)";
62  tesseract_scene_graph::Inertial::Ptr elem;
63  EXPECT_FALSE(runTest<tesseract_scene_graph::Inertial::Ptr>(
64  elem, &tesseract_urdf::parseInertial, str, tesseract_urdf::INERTIAL_ELEMENT_NAME.data()));
65  }
66 
67  {
68  std::string str = R"(<inertial>
69  <mass value="1.0"/>
70  </inertial>)";
71  tesseract_scene_graph::Inertial::Ptr elem;
72  EXPECT_FALSE(runTest<tesseract_scene_graph::Inertial::Ptr>(
73  elem, &tesseract_urdf::parseInertial, str, tesseract_urdf::INERTIAL_ELEMENT_NAME.data()));
74  }
75 
76  {
77  std::string str = R"(<inertial>
78  <mass value="a"/>
79  <inertia ixx="1.0" ixy="2.0" ixz="3.0" iyy="4.0" iyz="5.0" izz="6.0"/>
80  </inertial>)";
81  tesseract_scene_graph::Inertial::Ptr elem;
82  EXPECT_FALSE(runTest<tesseract_scene_graph::Inertial::Ptr>(
83  elem, &tesseract_urdf::parseInertial, str, tesseract_urdf::INERTIAL_ELEMENT_NAME.data()));
84  }
85 
86  {
87  std::string str = R"(<inertial>
88  <mass />
89  <inertia ixx="1.0" ixy="2.0" ixz="3.0" iyy="4.0" iyz="5.0" izz="6.0"/>
90  </inertial>)";
91  tesseract_scene_graph::Inertial::Ptr elem;
92  EXPECT_FALSE(runTest<tesseract_scene_graph::Inertial::Ptr>(
93  elem, &tesseract_urdf::parseInertial, str, tesseract_urdf::INERTIAL_ELEMENT_NAME.data()));
94  }
95 
96  {
97  std::string str = R"(<inertial>
98  <mass value="1.0"/>
99  <inertia ixx="a" ixy="2.0" ixz="3.0" iyy="4.0" iyz="5.0" izz="6.0"/>
100  </inertial>)";
101  tesseract_scene_graph::Inertial::Ptr elem;
102  EXPECT_FALSE(runTest<tesseract_scene_graph::Inertial::Ptr>(
103  elem, &tesseract_urdf::parseInertial, str, tesseract_urdf::INERTIAL_ELEMENT_NAME.data()));
104  }
105 
106  {
107  std::string str = R"(<inertial>
108  <mass value="1.0"/>
109  <inertia ixx="1.0" ixy="a" ixz="3.0" iyy="4.0" iyz="5.0" izz="6.0"/>
110  </inertial>)";
111  tesseract_scene_graph::Inertial::Ptr elem;
112  EXPECT_FALSE(runTest<tesseract_scene_graph::Inertial::Ptr>(
113  elem, &tesseract_urdf::parseInertial, str, tesseract_urdf::INERTIAL_ELEMENT_NAME.data()));
114  }
115 
116  {
117  std::string str = R"(<inertial>
118  <mass value="1.0"/>
119  <inertia ixx="1.0" ixy="2.0" ixz="a" iyy="4.0" iyz="5.0" izz="6.0"/>
120  </inertial>)";
121  tesseract_scene_graph::Inertial::Ptr elem;
122  EXPECT_FALSE(runTest<tesseract_scene_graph::Inertial::Ptr>(
123  elem, &tesseract_urdf::parseInertial, str, tesseract_urdf::INERTIAL_ELEMENT_NAME.data()));
124  }
125 
126  {
127  std::string str = R"(<inertial>
128  <mass value="1.0"/>
129  <inertia ixx="1.0" ixy="2.0" ixz="3.0" iyy="a" iyz="5.0" izz="6.0"/>
130  </inertial>)";
131  tesseract_scene_graph::Inertial::Ptr elem;
132  EXPECT_FALSE(runTest<tesseract_scene_graph::Inertial::Ptr>(
133  elem, &tesseract_urdf::parseInertial, str, tesseract_urdf::INERTIAL_ELEMENT_NAME.data()));
134  }
135 
136  {
137  std::string str = R"(<inertial>
138  <mass value="1.0"/>
139  <inertia ixx="1.0" ixy="2.0" ixz="3.0" iyy="4.0" iyz="a" izz="6.0"/>
140  </inertial>)";
141  tesseract_scene_graph::Inertial::Ptr elem;
142  EXPECT_FALSE(runTest<tesseract_scene_graph::Inertial::Ptr>(
143  elem, &tesseract_urdf::parseInertial, str, tesseract_urdf::INERTIAL_ELEMENT_NAME.data()));
144  }
145 
146  {
147  std::string str = R"(<inertial>
148  <mass value="1.0"/>
149  <inertia ixx="1.0" ixy="2.0" ixz="3.0" iyy="4.0" iyz="5.0" izz="a"/>
150  </inertial>)";
151  tesseract_scene_graph::Inertial::Ptr elem;
152  EXPECT_FALSE(runTest<tesseract_scene_graph::Inertial::Ptr>(
153  elem, &tesseract_urdf::parseInertial, str, tesseract_urdf::INERTIAL_ELEMENT_NAME.data()));
154  }
155 
156  {
157  std::string str = R"(<inertial>
158  <mass value="1.0"/>
159  <inertia ixy="2.0" ixz="3.0" iyy="4.0" iyz="5.0" izz="6.0"/>
160  </inertial>)";
161  tesseract_scene_graph::Inertial::Ptr elem;
162  EXPECT_FALSE(runTest<tesseract_scene_graph::Inertial::Ptr>(
163  elem, &tesseract_urdf::parseInertial, str, tesseract_urdf::INERTIAL_ELEMENT_NAME.data()));
164  }
165 
166  {
167  std::string str = R"(<inertial>
168  <mass value="1.0"/>
169  <inertia ixx="1.0" ixz="3.0" iyy="4.0" iyz="5.0" izz="6.0"/>
170  </inertial>)";
171  tesseract_scene_graph::Inertial::Ptr elem;
172  EXPECT_FALSE(runTest<tesseract_scene_graph::Inertial::Ptr>(
173  elem, &tesseract_urdf::parseInertial, str, tesseract_urdf::INERTIAL_ELEMENT_NAME.data()));
174  }
175 
176  {
177  std::string str = R"(<inertial>
178  <mass value="1.0"/>
179  <inertia ixx="1.0" ixy="2.0" iyy="4.0" iyz="5.0" izz="6.0"/>
180  </inertial>)";
181  tesseract_scene_graph::Inertial::Ptr elem;
182  EXPECT_FALSE(runTest<tesseract_scene_graph::Inertial::Ptr>(
183  elem, &tesseract_urdf::parseInertial, str, tesseract_urdf::INERTIAL_ELEMENT_NAME.data()));
184  }
185 
186  {
187  std::string str = R"(<inertial>
188  <mass value="1.0"/>
189  <inertia ixx="1.0" ixy="2.0" ixz="3.0" iyz="5.0" izz="6.0"/>
190  </inertial>)";
191  tesseract_scene_graph::Inertial::Ptr elem;
192  EXPECT_FALSE(runTest<tesseract_scene_graph::Inertial::Ptr>(
193  elem, &tesseract_urdf::parseInertial, str, tesseract_urdf::INERTIAL_ELEMENT_NAME.data()));
194  }
195 
196  {
197  std::string str = R"(<inertial>
198  <mass value="1.0"/>
199  <inertia ixx="1.0" ixy="2.0" ixz="3.0" iyy="4.0" izz="6.0"/>
200  </inertial>)";
201  tesseract_scene_graph::Inertial::Ptr elem;
202  EXPECT_FALSE(runTest<tesseract_scene_graph::Inertial::Ptr>(
203  elem, &tesseract_urdf::parseInertial, str, tesseract_urdf::INERTIAL_ELEMENT_NAME.data()));
204  }
205 
206  {
207  std::string str = R"(<inertial>
208  <mass value="1.0"/>
209  <inertia ixx="1.0" ixy="2.0" ixz="3.0" iyy="4.0" iyz="5.0"/>
210  </inertial>)";
211  tesseract_scene_graph::Inertial::Ptr elem;
212  EXPECT_FALSE(runTest<tesseract_scene_graph::Inertial::Ptr>(
213  elem, &tesseract_urdf::parseInertial, str, tesseract_urdf::INERTIAL_ELEMENT_NAME.data()));
214  }
215 
216  {
217  std::string str = R"(<inertial>
218  <mass value="1.0"/>
219  <inertia />
220  </inertial>)";
221  tesseract_scene_graph::Inertial::Ptr elem;
222  EXPECT_FALSE(runTest<tesseract_scene_graph::Inertial::Ptr>(
223  elem, &tesseract_urdf::parseInertial, str, tesseract_urdf::INERTIAL_ELEMENT_NAME.data()));
224  }
225 }
226 
227 TEST(TesseractURDFUnit, write_inertial) // NOLINT
228 {
229  {
230  tesseract_scene_graph::Inertial::Ptr inertial = std::make_shared<tesseract_scene_graph::Inertial>();
231  inertial->origin = Eigen::Isometry3d::Identity();
232  inertial->ixx = 1.0;
233  inertial->ixy = 2.0;
234  inertial->iyy = 3.0;
235  inertial->iyz = 4.0;
236  inertial->izz = 5.0;
237  inertial->ixz = 6.0;
238  std::string text;
239  EXPECT_EQ(0, writeTest<tesseract_scene_graph::Inertial::Ptr>(inertial, &tesseract_urdf::writeInertial, text));
240  EXPECT_NE(text, "");
241  }
242 
243  {
244  tesseract_scene_graph::Inertial::Ptr inertial = std::make_shared<tesseract_scene_graph::Inertial>();
245  inertial->origin = Eigen::Isometry3d::Identity();
246  inertial->origin.translation() = Eigen::Vector3d(1.0, 2.0, 3.0);
247  inertial->ixx = 1.0;
248  inertial->ixy = 2.0;
249  inertial->iyy = 3.0;
250  inertial->iyz = 4.0;
251  inertial->izz = 5.0;
252  inertial->ixz = 6.0;
253  std::string text;
254  EXPECT_EQ(0, writeTest<tesseract_scene_graph::Inertial::Ptr>(inertial, &tesseract_urdf::writeInertial, text));
255  EXPECT_NE(text, "");
256  }
257 
258  {
259  tesseract_scene_graph::Inertial::Ptr inertial = nullptr;
260  std::string text;
261  EXPECT_EQ(1, writeTest<tesseract_scene_graph::Inertial::Ptr>(inertial, &tesseract_urdf::writeInertial, text));
262  EXPECT_EQ(text, "");
263  }
264 }
TEST
TESSERACT_COMMON_IGNORE_WARNINGS_PUSH TESSERACT_COMMON_IGNORE_WARNINGS_POP TEST(TesseractURDFUnit, parse_inertial)
Definition: tesseract_urdf_inertial_unit.cpp:10
tesseract_urdf::INERTIAL_ELEMENT_NAME
static constexpr std::string_view INERTIAL_ELEMENT_NAME
Definition: inertial.h:45
tesseract_urdf::writeInertial
tinyxml2::XMLElement * writeInertial(const std::shared_ptr< const tesseract_scene_graph::Inertial > &inertial, tinyxml2::XMLDocument &doc)
Definition: inertial.cpp:89
TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
#define TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
EXPECT_TRUE
#define EXPECT_TRUE(args)
tesseract_urdf_common_unit.h
EXPECT_NEAR
#define EXPECT_NEAR(a, b, prec)
TESSERACT_COMMON_IGNORE_WARNINGS_POP
tesseract_scene_graph::Inertial::Ptr
std::shared_ptr< Inertial > Ptr
inertial.h
Parse inertial from xml string.
tesseract_urdf::parseInertial
std::shared_ptr< tesseract_scene_graph::Inertial > parseInertial(const tinyxml2::XMLElement *xml_element)
Parse xml element inertial.
Definition: inertial.cpp:41
macros.h
EXPECT_EQ
#define EXPECT_EQ(a, b)
EXPECT_FALSE
#define EXPECT_FALSE(args)

tesseract_urdf
Author(s): Levi Armstrong
autogenerated on Sun May 18 2025 03:02:07