tesseract_urdf_joint_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_scene_graph/joint.h>
8 #include <tesseract_urdf/joint.h>
9 #include "tesseract_urdf_common_unit.h"
10 
11 TEST(TesseractURDFUnit, parse_joint) // NOLINT
12 {
13  {
14  std::string str =
15  R"(<joint name="my_joint" type="floating" extra="0 0 0">
16  <origin xyz="0 0 1" rpy="0 0 3.1416"/>
17  <parent link="link1" extra="0 0 0"/>
18  <child link="link2" extra="0 0 0"/>
19  <calibration rising="0.0"/>
20  <dynamics damping="0.0" friction="0.0"/>
21  <limit effort="30" velocity="1.0" lower="-2.2" upper="0.7" />
22  <safety_controller k_velocity="10" k_position="15" soft_lower_limit="-2.0" soft_upper_limit="0.5" />
23  </joint>)";
24  tesseract_scene_graph::Joint::Ptr elem;
25  EXPECT_TRUE(runTest<tesseract_scene_graph::Joint::Ptr>(
26  elem, &tesseract_urdf::parseJoint, str, tesseract_urdf::JOINT_ELEMENT_NAME.data()));
27  EXPECT_TRUE(elem->getName() == "my_joint");
28  EXPECT_TRUE(elem->type == tesseract_scene_graph::JointType::FLOATING);
29  EXPECT_FALSE(elem->parent_to_joint_origin_transform.isApprox(Eigen::Isometry3d::Identity(), 1e-8));
30  EXPECT_TRUE(elem->axis.isApprox(Eigen::Vector3d(1, 0, 0), 1e-8));
31  EXPECT_TRUE(elem->parent_link_name == "link1");
32  EXPECT_TRUE(elem->child_link_name == "link2");
33  EXPECT_TRUE(elem->calibration != nullptr);
34  EXPECT_TRUE(elem->dynamics != nullptr);
35  EXPECT_TRUE(elem->limits == nullptr);
36  EXPECT_TRUE(elem->safety != nullptr);
37  EXPECT_TRUE(elem->mimic == nullptr);
38  }
39 
40  {
41  std::string str =
42  R"(<joint name="my_joint" type="revolute">
43  <origin xyz="0 0 1" rpy="0 0 3.1416"/>
44  <parent link="link1"/>
45  <child link="link2"/>
46  <calibration rising="0.0"/>
47  <dynamics damping="0.0" friction="0.0"/>
48  <limit effort="30" velocity="1.0" lower="-2.2" upper="0.7" />
49  <safety_controller k_velocity="10" k_position="15" soft_lower_limit="-2.0" soft_upper_limit="0.5" />
50  </joint>)";
51  tesseract_scene_graph::Joint::Ptr elem;
52  EXPECT_TRUE(runTest<tesseract_scene_graph::Joint::Ptr>(
53  elem, &tesseract_urdf::parseJoint, str, tesseract_urdf::JOINT_ELEMENT_NAME.data()));
54  EXPECT_TRUE(elem->getName() == "my_joint");
55  EXPECT_TRUE(elem->type == tesseract_scene_graph::JointType::REVOLUTE);
56  EXPECT_FALSE(elem->parent_to_joint_origin_transform.isApprox(Eigen::Isometry3d::Identity(), 1e-8));
57  EXPECT_TRUE(elem->axis.isApprox(Eigen::Vector3d(1, 0, 0), 1e-8));
58  EXPECT_TRUE(elem->parent_link_name == "link1");
59  EXPECT_TRUE(elem->child_link_name == "link2");
60  EXPECT_TRUE(elem->calibration != nullptr);
61  EXPECT_TRUE(elem->dynamics != nullptr);
62  EXPECT_TRUE(elem->limits != nullptr);
63  EXPECT_TRUE(elem->safety != nullptr);
64  EXPECT_TRUE(elem->mimic == nullptr);
65  }
66 
67  {
68  std::string str = R"(<joint name="my_joint" type="revolute">
69  <axis xyz="0 0 1"/>
70  <parent link="link1"/>
71  <child link="link2"/>
72  <limit effort="30" velocity="1.0" lower="-2.2" upper="0.7" />
73  </joint>)";
74  tesseract_scene_graph::Joint::Ptr elem;
75  EXPECT_TRUE(runTest<tesseract_scene_graph::Joint::Ptr>(
76  elem, &tesseract_urdf::parseJoint, str, tesseract_urdf::JOINT_ELEMENT_NAME.data()));
77  EXPECT_TRUE(elem->getName() == "my_joint");
78  EXPECT_TRUE(elem->type == tesseract_scene_graph::JointType::REVOLUTE);
79  EXPECT_TRUE(elem->parent_to_joint_origin_transform.isApprox(Eigen::Isometry3d::Identity(), 1e-8));
80  EXPECT_TRUE(elem->axis.isApprox(Eigen::Vector3d(0, 0, 1), 1e-8));
81  EXPECT_TRUE(elem->parent_link_name == "link1");
82  EXPECT_TRUE(elem->child_link_name == "link2");
83  EXPECT_TRUE(elem->calibration == nullptr);
84  EXPECT_TRUE(elem->dynamics == nullptr);
85  EXPECT_TRUE(elem->limits != nullptr);
86  EXPECT_TRUE(elem->safety == nullptr);
87  EXPECT_TRUE(elem->mimic == nullptr);
88  }
89 
90  {
91  std::string str = R"(<joint name="my_joint" type="continuous">
92  <axis xyz="0 0 1"/>
93  <parent link="link1"/>
94  <child link="link2"/>
95  <limit effort="30" velocity="1.0"/>
96  </joint>)";
97  tesseract_scene_graph::Joint::Ptr elem;
98  EXPECT_TRUE(runTest<tesseract_scene_graph::Joint::Ptr>(
99  elem, &tesseract_urdf::parseJoint, str, tesseract_urdf::JOINT_ELEMENT_NAME.data()));
100  EXPECT_TRUE(elem->getName() == "my_joint");
101  EXPECT_TRUE(elem->type == tesseract_scene_graph::JointType::CONTINUOUS);
102  EXPECT_TRUE(elem->parent_to_joint_origin_transform.isApprox(Eigen::Isometry3d::Identity(), 1e-8));
103  EXPECT_TRUE(elem->axis.isApprox(Eigen::Vector3d(0, 0, 1), 1e-8));
104  EXPECT_TRUE(elem->parent_link_name == "link1");
105  EXPECT_TRUE(elem->child_link_name == "link2");
106  EXPECT_TRUE(elem->calibration == nullptr);
107  EXPECT_TRUE(elem->dynamics == nullptr);
108  EXPECT_TRUE(elem->limits != nullptr);
109  EXPECT_TRUE(elem->safety == nullptr);
110  EXPECT_TRUE(elem->mimic == nullptr);
111  }
112 
113  {
114  std::string str = R"(<joint name="my_joint" type="continuous">
115  <axis xyz="0 0 1"/>
116  <parent link="link1"/>
117  <child link="link2"/>
118  </joint>)";
119  tesseract_scene_graph::Joint::Ptr elem;
120  EXPECT_TRUE(runTest<tesseract_scene_graph::Joint::Ptr>(
121  elem, &tesseract_urdf::parseJoint, str, tesseract_urdf::JOINT_ELEMENT_NAME.data()));
122  EXPECT_TRUE(elem->getName() == "my_joint");
123  EXPECT_TRUE(elem->type == tesseract_scene_graph::JointType::CONTINUOUS);
124  EXPECT_TRUE(elem->parent_to_joint_origin_transform.isApprox(Eigen::Isometry3d::Identity(), 1e-8));
125  EXPECT_TRUE(elem->axis.isApprox(Eigen::Vector3d(0, 0, 1), 1e-8));
126  EXPECT_TRUE(elem->parent_link_name == "link1");
127  EXPECT_TRUE(elem->child_link_name == "link2");
128  EXPECT_TRUE(elem->calibration == nullptr);
129  EXPECT_TRUE(elem->dynamics == nullptr);
130  EXPECT_TRUE(elem->limits != nullptr);
131  EXPECT_TRUE(elem->safety == nullptr);
132  EXPECT_TRUE(elem->mimic == nullptr);
133  }
134 
135  {
136  std::string str = R"(<joint name="my_joint" type="fixed">
137  <axis xyz="0 0 1"/>
138  <parent link="link1"/>
139  <child link="link2"/>
140  <limit effort="30" velocity="1.0" lower="-2.2" upper="0.7" />
141  </joint>)";
142  tesseract_scene_graph::Joint::Ptr elem;
143  EXPECT_TRUE(runTest<tesseract_scene_graph::Joint::Ptr>(
144  elem, &tesseract_urdf::parseJoint, str, tesseract_urdf::JOINT_ELEMENT_NAME.data()));
145  EXPECT_TRUE(elem->getName() == "my_joint");
146  EXPECT_TRUE(elem->type == tesseract_scene_graph::JointType::FIXED);
147  EXPECT_TRUE(elem->parent_to_joint_origin_transform.isApprox(Eigen::Isometry3d::Identity(), 1e-8));
148  EXPECT_TRUE(elem->axis.isApprox(Eigen::Vector3d(1, 0, 0), 1e-8));
149  EXPECT_TRUE(elem->parent_link_name == "link1");
150  EXPECT_TRUE(elem->child_link_name == "link2");
151  EXPECT_TRUE(elem->calibration == nullptr);
152  EXPECT_TRUE(elem->dynamics == nullptr);
153  EXPECT_TRUE(elem->limits == nullptr);
154  EXPECT_TRUE(elem->safety == nullptr);
155  EXPECT_TRUE(elem->mimic == nullptr);
156  }
157 
158  {
159  std::string str = R"(<joint name="my_joint" type="prismatic">
160  <axis xyz="0 0 -1"/>
161  <parent link="link1"/>
162  <child link="link2"/>
163  <limit effort="30" velocity="1.0" lower="-2.2" upper="0.7" />
164  </joint>)";
165  tesseract_scene_graph::Joint::Ptr elem;
166  EXPECT_TRUE(runTest<tesseract_scene_graph::Joint::Ptr>(
167  elem, &tesseract_urdf::parseJoint, str, tesseract_urdf::JOINT_ELEMENT_NAME.data()));
168  EXPECT_TRUE(elem->getName() == "my_joint");
169  EXPECT_TRUE(elem->type == tesseract_scene_graph::JointType::PRISMATIC);
170  EXPECT_TRUE(elem->parent_to_joint_origin_transform.isApprox(Eigen::Isometry3d::Identity(), 1e-8));
171  EXPECT_TRUE(elem->axis.isApprox(Eigen::Vector3d(0, 0, -1), 1e-8));
172  EXPECT_TRUE(elem->parent_link_name == "link1");
173  EXPECT_TRUE(elem->child_link_name == "link2");
174  EXPECT_TRUE(elem->calibration == nullptr);
175  EXPECT_TRUE(elem->dynamics == nullptr);
176  EXPECT_TRUE(elem->limits != nullptr);
177  EXPECT_TRUE(elem->safety == nullptr);
178  EXPECT_TRUE(elem->mimic == nullptr);
179  }
180 
181  {
182  std::string str = R"(<joint name="my_joint" type="revolute">
183  <axis xyz="0 0 -1"/>
184  <parent link="link1"/>
185  <child link="link2"/>
186  <limit effort="30" velocity="1.0" lower="-2.2" upper="0.7" />
187  </joint>)";
188  tesseract_scene_graph::Joint::Ptr elem;
189  EXPECT_TRUE(runTest<tesseract_scene_graph::Joint::Ptr>(
190  elem, &tesseract_urdf::parseJoint, str, tesseract_urdf::JOINT_ELEMENT_NAME.data()));
191  EXPECT_TRUE(elem->getName() == "my_joint");
192  EXPECT_TRUE(elem->type == tesseract_scene_graph::JointType::REVOLUTE);
193  EXPECT_TRUE(elem->parent_to_joint_origin_transform.isApprox(Eigen::Isometry3d::Identity(), 1e-8));
194  EXPECT_TRUE(elem->axis.isApprox(Eigen::Vector3d(0, 0, -1), 1e-8));
195  EXPECT_TRUE(elem->parent_link_name == "link1");
196  EXPECT_TRUE(elem->child_link_name == "link2");
197  EXPECT_TRUE(elem->calibration == nullptr);
198  EXPECT_TRUE(elem->dynamics == nullptr);
199  EXPECT_TRUE(elem->limits != nullptr);
200  EXPECT_TRUE(elem->safety == nullptr);
201  EXPECT_TRUE(elem->mimic == nullptr);
202  }
203 
204  {
205  std::string str = R"(<joint name="my_joint" type="planar">
206  <axis xyz="0 0 -1.0"/>
207  <parent link="link1"/>
208  <child link="link2"/>
209  <limit effort="30" velocity="1.0" lower="-2.2" upper="0.7" />
210  </joint>)";
211  tesseract_scene_graph::Joint::Ptr elem;
212  EXPECT_TRUE(runTest<tesseract_scene_graph::Joint::Ptr>(
213  elem, &tesseract_urdf::parseJoint, str, tesseract_urdf::JOINT_ELEMENT_NAME.data()));
214  EXPECT_TRUE(elem->getName() == "my_joint");
215  EXPECT_TRUE(elem->type == tesseract_scene_graph::JointType::PLANAR);
216  EXPECT_TRUE(elem->parent_to_joint_origin_transform.isApprox(Eigen::Isometry3d::Identity(), 1e-8));
217  EXPECT_TRUE(elem->axis.isApprox(Eigen::Vector3d(0, 0, -1), 1e-8));
218  EXPECT_TRUE(elem->parent_link_name == "link1");
219  EXPECT_TRUE(elem->child_link_name == "link2");
220  EXPECT_TRUE(elem->calibration == nullptr);
221  EXPECT_TRUE(elem->dynamics == nullptr);
222  EXPECT_TRUE(elem->limits == nullptr);
223  EXPECT_TRUE(elem->safety == nullptr);
224  EXPECT_TRUE(elem->mimic == nullptr);
225  }
226 
227  {
228  std::string str = R"(<joint name="my_joint">
229  <parent link="link1"/>
230  <child link="link2"/>
231  </joint>)";
232  tesseract_scene_graph::Joint::Ptr elem;
233  EXPECT_FALSE(runTest<tesseract_scene_graph::Joint::Ptr>(
234  elem, &tesseract_urdf::parseJoint, str, tesseract_urdf::JOINT_ELEMENT_NAME.data()));
235  }
236 
237  {
238  std::string str = R"(<joint type="planar">
239  <parent link="link1"/>
240  <child link="link2"/>
241  </joint>)";
242  tesseract_scene_graph::Joint::Ptr elem;
243  EXPECT_FALSE(runTest<tesseract_scene_graph::Joint::Ptr>(
244  elem, &tesseract_urdf::parseJoint, str, tesseract_urdf::JOINT_ELEMENT_NAME.data()));
245  }
246 
247  {
248  std::string str = R"(<joint name="my_joint" type="planar">
249  <child link="link2"/>
250  </joint>)";
251  tesseract_scene_graph::Joint::Ptr elem;
252  EXPECT_FALSE(runTest<tesseract_scene_graph::Joint::Ptr>(
253  elem, &tesseract_urdf::parseJoint, str, tesseract_urdf::JOINT_ELEMENT_NAME.data()));
254  }
255 
256  {
257  std::string str = R"(<joint name="my_joint" type="planar">
258  <parent link="link1"/>
259  </joint>)";
260  tesseract_scene_graph::Joint::Ptr elem;
261  EXPECT_FALSE(runTest<tesseract_scene_graph::Joint::Ptr>(
262  elem, &tesseract_urdf::parseJoint, str, tesseract_urdf::JOINT_ELEMENT_NAME.data()));
263  }
264 
265  {
266  std::string str = R"(<joint name="my_joint" type="planar">
267  <axis />
268  <parent link="link1"/>
269  <child link="link2"/>
270  <limit effort="30" velocity="1.0" lower="-2.2" upper="0.7" />
271  </joint>)";
272  tesseract_scene_graph::Joint::Ptr elem;
273  EXPECT_FALSE(runTest<tesseract_scene_graph::Joint::Ptr>(
274  elem, &tesseract_urdf::parseJoint, str, tesseract_urdf::JOINT_ELEMENT_NAME.data()));
275  }
276 
277  {
278  std::string str = R"(<joint name="my_joint" type="planar">
279  <axis xyz="0 0 -1.0 4"/>
280  <parent link="link1"/>
281  <child link="link2"/>
282  <limit effort="30" velocity="1.0" lower="-2.2" upper="0.7" />
283  </joint>)";
284  tesseract_scene_graph::Joint::Ptr elem;
285  EXPECT_FALSE(runTest<tesseract_scene_graph::Joint::Ptr>(
286  elem, &tesseract_urdf::parseJoint, str, tesseract_urdf::JOINT_ELEMENT_NAME.data()));
287  }
288 
289  {
290  std::string str = R"(<joint name="my_joint" type="planar">
291  <axis xyz="a 0 -1.0"/>
292  <parent link="link1"/>
293  <child link="link2"/>
294  <limit effort="30" velocity="1.0" lower="-2.2" upper="0.7" />
295  </joint>)";
296  tesseract_scene_graph::Joint::Ptr elem;
297  EXPECT_FALSE(runTest<tesseract_scene_graph::Joint::Ptr>(
298  elem, &tesseract_urdf::parseJoint, str, tesseract_urdf::JOINT_ELEMENT_NAME.data()));
299  }
300 
301  {
302  std::string str = R"(<joint name="my_joint" type="planar">
303  <axis xyz="0 a -1.0"/>
304  <parent link="link1"/>
305  <child link="link2"/>
306  <limit effort="30" velocity="1.0" lower="-2.2" upper="0.7" />
307  </joint>)";
308  tesseract_scene_graph::Joint::Ptr elem;
309  EXPECT_FALSE(runTest<tesseract_scene_graph::Joint::Ptr>(
310  elem, &tesseract_urdf::parseJoint, str, tesseract_urdf::JOINT_ELEMENT_NAME.data()));
311  }
312 
313  {
314  std::string str = R"(<joint name="my_joint" type="planar">
315  <axis xyz="0 0 a"/>
316  <parent link="link1"/>
317  <child link="link2"/>
318  <limit effort="30" velocity="1.0" lower="-2.2" upper="0.7" />
319  </joint>)";
320  tesseract_scene_graph::Joint::Ptr elem;
321  EXPECT_FALSE(runTest<tesseract_scene_graph::Joint::Ptr>(
322  elem, &tesseract_urdf::parseJoint, str, tesseract_urdf::JOINT_ELEMENT_NAME.data()));
323  }
324 
325  {
326  std::string str =
327  R"(<joint name="my_joint" type="floating" extra="0 0 0">
328  <origin xyz="0 0 1 4" rpy="0 0 3.1416"/>
329  <parent link="link1" extra="0 0 0"/>
330  <child link="link2" extra="0 0 0"/>
331  <calibration rising="0.0"/>
332  <dynamics damping="0.0" friction="0.0"/>
333  <limit effort="30" velocity="1.0" lower="-2.2" upper="0.7" />
334  <safety_controller k_velocity="10" k_position="15" soft_lower_limit="-2.0" soft_upper_limit="0.5" />
335  </joint>)";
336  tesseract_scene_graph::Joint::Ptr elem;
337  EXPECT_FALSE(runTest<tesseract_scene_graph::Joint::Ptr>(
338  elem, &tesseract_urdf::parseJoint, str, tesseract_urdf::JOINT_ELEMENT_NAME.data()));
339  }
340 
341  {
342  std::string str =
343  R"(<joint name="my_joint" type="floating" extra="0 0 0">
344  <origin xyz="0 0 1" rpy="0 0 3.1416"/>
345  <parent extra="0 0 0"/>
346  <child link="link2" extra="0 0 0"/>
347  <calibration rising="0.0"/>
348  <dynamics damping="0.0" friction="0.0"/>
349  <limit effort="30" velocity="1.0" lower="-2.2" upper="0.7" />
350  <safety_controller k_velocity="10" k_position="15" soft_lower_limit="-2.0" soft_upper_limit="0.5" />
351  </joint>)";
352  tesseract_scene_graph::Joint::Ptr elem;
353  EXPECT_FALSE(runTest<tesseract_scene_graph::Joint::Ptr>(
354  elem, &tesseract_urdf::parseJoint, str, tesseract_urdf::JOINT_ELEMENT_NAME.data()));
355  }
356 
357  {
358  std::string str =
359  R"(<joint name="my_joint" type="floating" extra="0 0 0">
360  <origin xyz="0 0 1" rpy="0 0 3.1416"/>
361  <parent link="link1" extra="0 0 0"/>
362  <child extra="0 0 0"/>
363  <calibration rising="0.0"/>
364  <dynamics damping="0.0" friction="0.0"/>
365  <limit effort="30" velocity="1.0" lower="-2.2" upper="0.7" />
366  <safety_controller k_velocity="10" k_position="15" soft_lower_limit="-2.0" soft_upper_limit="0.5" />
367  </joint>)";
368  tesseract_scene_graph::Joint::Ptr elem;
369  EXPECT_FALSE(runTest<tesseract_scene_graph::Joint::Ptr>(
370  elem, &tesseract_urdf::parseJoint, str, tesseract_urdf::JOINT_ELEMENT_NAME.data()));
371  }
372 
373  {
374  std::string str =
375  R"(<joint name="my_joint" type="error" extra="0 0 0">
376  <origin xyz="0 0 1" rpy="0 0 3.1416"/>
377  <parent link="link1" extra="0 0 0"/>
378  <child link="link2" extra="0 0 0"/>
379  <calibration rising="0.0"/>
380  <dynamics damping="0.0" friction="0.0"/>
381  <limit effort="30" velocity="1.0" lower="-2.2" upper="0.7" />
382  <safety_controller k_velocity="10" k_position="15" soft_lower_limit="-2.0" soft_upper_limit="0.5" />
383  </joint>)";
384  tesseract_scene_graph::Joint::Ptr elem;
385  EXPECT_FALSE(runTest<tesseract_scene_graph::Joint::Ptr>(
386  elem, &tesseract_urdf::parseJoint, str, tesseract_urdf::JOINT_ELEMENT_NAME.data()));
387  }
388 
389  {
390  std::string str =
391  R"(<joint name="my_joint" type="floating" extra="0 0 0">
392  <origin xyz="0 0 1" rpy="0 0 3.1416"/>
393  <parent link="link1" extra="0 0 0"/>
394  <child link="link2" extra="0 0 0"/>
395  <calibration/>
396  <dynamics damping="0.0" friction="0.0"/>
397  <limit effort="30" velocity="1.0" lower="-2.2" upper="0.7" />
398  <safety_controller k_velocity="10" k_position="15" soft_lower_limit="-2.0" soft_upper_limit="0.5" />
399  </joint>)";
400  tesseract_scene_graph::Joint::Ptr elem;
401  EXPECT_FALSE(runTest<tesseract_scene_graph::Joint::Ptr>(
402  elem, &tesseract_urdf::parseJoint, str, tesseract_urdf::JOINT_ELEMENT_NAME.data()));
403  }
404 
405  {
406  std::string str =
407  R"(<joint name="my_joint" type="floating" extra="0 0 0">
408  <origin xyz="0 0 1" rpy="0 0 3.1416"/>
409  <parent link="link1" extra="0 0 0"/>
410  <child link="link2" extra="0 0 0"/>
411  <calibration rising="0.0"/>
412  <dynamics />
413  <limit effort="30" velocity="1.0" lower="-2.2" upper="0.7" />
414  <safety_controller k_velocity="10" k_position="15" soft_lower_limit="-2.0" soft_upper_limit="0.5" />
415  </joint>)";
416  tesseract_scene_graph::Joint::Ptr elem;
417  EXPECT_FALSE(runTest<tesseract_scene_graph::Joint::Ptr>(
418  elem, &tesseract_urdf::parseJoint, str, tesseract_urdf::JOINT_ELEMENT_NAME.data()));
419  }
420 
421  {
422  std::string str =
423  R"(<joint name="my_joint" type="revolute" extra="0 0 0">
424  <axis xyz="0 0 1"/>
425  <origin xyz="0 0 1" rpy="0 0 3.1416"/>
426  <parent link="link1" extra="0 0 0"/>
427  <child link="link2" extra="0 0 0"/>
428  <calibration rising="0.0"/>
429  <dynamics damping="0.0" friction="0.0"/>
430  <limit />
431  <safety_controller k_velocity="10" k_position="15" soft_lower_limit="-2.0" soft_upper_limit="0.5" />
432  </joint>)";
433  tesseract_scene_graph::Joint::Ptr elem;
434  EXPECT_FALSE(runTest<tesseract_scene_graph::Joint::Ptr>(
435  elem, &tesseract_urdf::parseJoint, str, tesseract_urdf::JOINT_ELEMENT_NAME.data()));
436  }
437 
438  {
439  std::string str =
440  R"(<joint name="my_joint" type="floating" extra="0 0 0">
441  <origin xyz="0 0 1" rpy="0 0 3.1416"/>
442  <parent link="link1" extra="0 0 0"/>
443  <child link="link2" extra="0 0 0"/>
444  <calibration rising="0.0"/>
445  <dynamics damping="0.0" friction="0.0"/>
446  <limit effort="30" velocity="1.0" lower="-2.2" upper="0.7" />
447  <safety_controller/>
448  </joint>)";
449  tesseract_scene_graph::Joint::Ptr elem;
450  EXPECT_FALSE(runTest<tesseract_scene_graph::Joint::Ptr>(
451  elem, &tesseract_urdf::parseJoint, str, tesseract_urdf::JOINT_ELEMENT_NAME.data()));
452  }
453 
454  {
455  std::string str = R"(<joint name="my_joint" type="mimic">
456  <axis xyz="0 0 -1"/>
457  <parent link="link1"/>
458  <child link="link2"/>
459  <limit effort="30" velocity="1.0" lower="-2.2" upper="0.7" />
460  <mimic multiplier="a" offset="1" />
461  </joint>)";
462  tesseract_scene_graph::Joint::Ptr elem;
463  EXPECT_FALSE(runTest<tesseract_scene_graph::Joint::Ptr>(
464  elem, &tesseract_urdf::parseJoint, str, tesseract_urdf::JOINT_ELEMENT_NAME.data()));
465  }
466 
467  {
468  std::string str = R"(<joint name="my_joint" type="revolute">
469  <axis xyz="0 0 -1"/>
470  <parent link="link1"/>
471  <child link="link2"/>
472  </joint>)";
473  tesseract_scene_graph::Joint::Ptr elem;
474  EXPECT_FALSE(runTest<tesseract_scene_graph::Joint::Ptr>(
475  elem, &tesseract_urdf::parseJoint, str, tesseract_urdf::JOINT_ELEMENT_NAME.data()));
476  }
477 }
478 
479 TEST(TesseractURDFUnit, write_joint) // NOLINT
480 {
481  { // trigger nullptr input
482  tesseract_scene_graph::Joint::Ptr joint = nullptr;
483  std::string text;
484  EXPECT_EQ(1, writeTest<tesseract_scene_graph::Joint::Ptr>(joint, &tesseract_urdf::writeJoint, text));
485  EXPECT_EQ(text, "");
486  }
487 
488  { // trigger type planar, set joint axis
489  tesseract_scene_graph::Joint::Ptr joint = std::make_shared<tesseract_scene_graph::Joint>("joint_0");
490  joint->type = tesseract_scene_graph::JointType::PLANAR;
491  joint->axis = Eigen::Vector3d::UnitX();
492  std::string text;
493  EXPECT_EQ(0, writeTest<tesseract_scene_graph::Joint::Ptr>(joint, &tesseract_urdf::writeJoint, text));
494  EXPECT_NE(text, "");
495  }
496 
497  { // trigger type floating
498  tesseract_scene_graph::Joint::Ptr joint = std::make_shared<tesseract_scene_graph::Joint>("joint_0");
499  joint->type = tesseract_scene_graph::JointType::FLOATING;
500  std::string text;
501  EXPECT_EQ(0, writeTest<tesseract_scene_graph::Joint::Ptr>(joint, &tesseract_urdf::writeJoint, text));
502  EXPECT_NE(text, "");
503  }
504 
505  { // trigger type revolute; set joint axis, joint limits, joint safety, joint cal, mimic joint, dynamics
506  tesseract_scene_graph::Joint::Ptr joint = std::make_shared<tesseract_scene_graph::Joint>("joint_0");
507  joint->type = tesseract_scene_graph::JointType::REVOLUTE;
508  joint->axis = Eigen::Vector3d::UnitY();
509  joint->limits = std::make_shared<tesseract_scene_graph::JointLimits>();
510  joint->limits->lower = 1.0;
511  joint->limits->upper = 2.0;
512  joint->safety = std::make_shared<tesseract_scene_graph::JointSafety>();
513  joint->calibration = std::make_shared<tesseract_scene_graph::JointCalibration>();
514  joint->mimic = std::make_shared<tesseract_scene_graph::JointMimic>();
515  joint->dynamics = std::make_shared<tesseract_scene_graph::JointDynamics>();
516  std::string text;
517  EXPECT_EQ(0, writeTest<tesseract_scene_graph::Joint::Ptr>(joint, &tesseract_urdf::writeJoint, text));
518  EXPECT_NE(text, "");
519  }
520 
521  { // trigger type revolute; set joint axis, joint limits, joint safety, joint cal, mimic joint, dynamics, origin not
522  // identity
523  tesseract_scene_graph::Joint::Ptr joint = std::make_shared<tesseract_scene_graph::Joint>("joint_0");
524  joint->parent_to_joint_origin_transform.translation() = Eigen::Vector3d(1.0, 2.0, 3.0);
525  joint->type = tesseract_scene_graph::JointType::REVOLUTE;
526  joint->axis = Eigen::Vector3d::UnitY();
527  joint->limits = std::make_shared<tesseract_scene_graph::JointLimits>();
528  joint->limits->lower = 1.0;
529  joint->limits->upper = 2.0;
530  joint->safety = std::make_shared<tesseract_scene_graph::JointSafety>();
531  joint->calibration = std::make_shared<tesseract_scene_graph::JointCalibration>();
532  joint->mimic = std::make_shared<tesseract_scene_graph::JointMimic>();
533  joint->dynamics = std::make_shared<tesseract_scene_graph::JointDynamics>();
534  std::string text;
535  EXPECT_EQ(0, writeTest<tesseract_scene_graph::Joint::Ptr>(joint, &tesseract_urdf::writeJoint, text));
536  EXPECT_NE(text, "");
537  }
538 
539  { // trigger type continuous
540  tesseract_scene_graph::Joint::Ptr joint = std::make_shared<tesseract_scene_graph::Joint>("joint_0");
541  joint->type = tesseract_scene_graph::JointType::CONTINUOUS;
542  joint->axis = Eigen::Vector3d::UnitZ();
543  joint->limits = std::make_shared<tesseract_scene_graph::JointLimits>();
544  joint->limits->acceleration = 1.0;
545  std::string text;
546  EXPECT_EQ(0, writeTest<tesseract_scene_graph::Joint::Ptr>(joint, &tesseract_urdf::writeJoint, text));
547  EXPECT_NE(text, "");
548  }
549 
550  { // trigger type prismatic
551  tesseract_scene_graph::Joint::Ptr joint = std::make_shared<tesseract_scene_graph::Joint>("joint_0");
552  joint->type = tesseract_scene_graph::JointType::PRISMATIC;
553  joint->limits = std::make_shared<tesseract_scene_graph::JointLimits>();
554  joint->limits->lower = 1.0;
555  joint->limits->upper = 2.0;
556  joint->axis = Eigen::Vector3d::Ones();
557  std::string text;
558  EXPECT_EQ(0, writeTest<tesseract_scene_graph::Joint::Ptr>(joint, &tesseract_urdf::writeJoint, text));
559  EXPECT_NE(text, "");
560  }
561 
562  { // trigger type fixed
563  tesseract_scene_graph::Joint::Ptr joint = std::make_shared<tesseract_scene_graph::Joint>("joint_0");
564  joint->type = tesseract_scene_graph::JointType::FIXED;
565  std::string text;
566  EXPECT_EQ(0, writeTest<tesseract_scene_graph::Joint::Ptr>(joint, &tesseract_urdf::writeJoint, text));
567  EXPECT_NE(text, "");
568  }
569 
570  { // trigger no joint limits
571  tesseract_scene_graph::Joint::Ptr joint = std::make_shared<tesseract_scene_graph::Joint>("joint_0");
572  joint->type = tesseract_scene_graph::JointType::PRISMATIC;
573  joint->limits = nullptr;
574  joint->axis = Eigen::Vector3d::Ones();
575  std::string text;
576  EXPECT_EQ(1, writeTest<tesseract_scene_graph::Joint::Ptr>(joint, &tesseract_urdf::writeJoint, text));
577  EXPECT_EQ(text, "");
578  }
579 
580  { // trigger no joint limits upper/lower equal zero error
581  tesseract_scene_graph::Joint::Ptr joint = std::make_shared<tesseract_scene_graph::Joint>("joint_0");
582  joint->type = tesseract_scene_graph::JointType::PRISMATIC;
583  joint->limits = std::make_shared<tesseract_scene_graph::JointLimits>();
584  joint->limits->lower = 0.0;
585  joint->limits->upper = 0.0;
586  joint->axis = Eigen::Vector3d::Ones();
587  std::string text;
588  EXPECT_EQ(1, writeTest<tesseract_scene_graph::Joint::Ptr>(joint, &tesseract_urdf::writeJoint, text));
589  }
590 }
tesseract_scene_graph::JointType::REVOLUTE
@ REVOLUTE
tesseract_urdf::JOINT_ELEMENT_NAME
static constexpr std::string_view JOINT_ELEMENT_NAME
Definition: joint.h:45
tesseract_scene_graph::JointType::FLOATING
@ FLOATING
TEST
TESSERACT_COMMON_IGNORE_WARNINGS_PUSH TESSERACT_COMMON_IGNORE_WARNINGS_POP TEST(TesseractURDFUnit, parse_joint)
Definition: tesseract_urdf_joint_unit.cpp:11
joint.h
TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
#define TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
EXPECT_TRUE
#define EXPECT_TRUE(args)
joint.h
Parse joint from xml string.
tesseract_scene_graph::JointType::PLANAR
@ PLANAR
tesseract_urdf_common_unit.h
tesseract_scene_graph::JointType::PRISMATIC
@ PRISMATIC
TESSERACT_COMMON_IGNORE_WARNINGS_POP
tesseract_urdf::parseJoint
std::shared_ptr< tesseract_scene_graph::Joint > parseJoint(const tinyxml2::XMLElement *xml_element)
Parse xml element joint.
Definition: joint.cpp:48
tesseract_scene_graph::JointType::CONTINUOUS
@ CONTINUOUS
tesseract_scene_graph::Joint::Ptr
std::shared_ptr< Joint > Ptr
tesseract_urdf::writeJoint
tinyxml2::XMLElement * writeJoint(const std::shared_ptr< const tesseract_scene_graph::Joint > &joint, tinyxml2::XMLDocument &doc)
Definition: joint.cpp:233
macros.h
EXPECT_EQ
#define EXPECT_EQ(a, b)
EXPECT_FALSE
#define EXPECT_FALSE(args)
tesseract_scene_graph::JointType::FIXED
@ FIXED

tesseract_urdf
Author(s): Levi Armstrong
autogenerated on Thu Apr 24 2025 03:10:44