tesseract_urdf_origin_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/origin.h>
8 #include "tesseract_urdf_common_unit.h"
9 
15 Eigen::Quaterniond fromRPY(double roll, double pitch, double yaw)
16 {
17  double phi = roll / 2.0;
18  double the = pitch / 2.0;
19  double psi = yaw / 2.0;
20 
21  double x{ 0 }, y{ 0 }, z{ 0 }, w{ 0 };
22  x = std::sin(phi) * std::cos(the) * std::cos(psi) - std::cos(phi) * std::sin(the) * std::sin(psi);
23  y = std::cos(phi) * std::sin(the) * std::cos(psi) + std::sin(phi) * std::cos(the) * std::sin(psi);
24  z = std::cos(phi) * std::cos(the) * std::sin(psi) - std::sin(phi) * std::sin(the) * std::cos(psi);
25  w = std::cos(phi) * std::cos(the) * std::cos(psi) + std::sin(phi) * std::sin(the) * std::sin(psi);
26 
27  return Eigen::Quaterniond{ w, x, y, z };
28 }
29 
30 TEST(TesseractURDFUnit, parse_origin) // NOLINT
31 {
32  {
33  std::string str = R"(<origin xyz="0 0 0" rpy="0 0 0"/>)";
34  Eigen::Isometry3d origin;
35  EXPECT_TRUE(runTest<Eigen::Isometry3d>(
36  origin, &tesseract_urdf::parseOrigin, str, tesseract_urdf::ORIGIN_ELEMENT_NAME.data()));
37  EXPECT_TRUE(origin.isApprox(Eigen::Isometry3d::Identity(), 1e-8));
38  }
39 
40  {
41  std::string str = R"(<origin xyz="0 0 0" wxyz="1 0 0 0"/>)";
42  Eigen::Isometry3d origin;
43  EXPECT_TRUE(runTest<Eigen::Isometry3d>(
44  origin, &tesseract_urdf::parseOrigin, str, tesseract_urdf::ORIGIN_ELEMENT_NAME.data()));
45  EXPECT_TRUE(origin.isApprox(Eigen::Isometry3d::Identity(), 1e-8));
46  }
47 
48  {
49  std::string str = R"(<origin xyz="0 0 0" wxyz="0.8719632 0.247934 0.177848 0.3828563"/>)";
50  Eigen::Isometry3d origin;
51  Eigen::Isometry3d check = Eigen::Isometry3d::Identity();
52  check.linear() << 0.6435823, -0.5794841, 0.5000000, 0.7558624, 0.5838996, -0.2961981, -0.1203077, 0.5685591,
53  0.8137977;
54  EXPECT_TRUE(runTest<Eigen::Isometry3d>(
55  origin, &tesseract_urdf::parseOrigin, str, tesseract_urdf::ORIGIN_ELEMENT_NAME.data()));
56  EXPECT_TRUE(origin.isApprox(check, 1e-6));
57  }
58 
59  {
60  std::string str = R"(<origin xyz="0 0 0" rpy="0.3490659 0.5235988 0.7330383"/>)";
61  Eigen::Isometry3d origin;
62  Eigen::Isometry3d check = Eigen::Isometry3d::Identity();
63  check.linear() = fromRPY(0.3490659, 0.5235988, 0.7330383).toRotationMatrix();
64  EXPECT_TRUE(runTest<Eigen::Isometry3d>(
65  origin, &tesseract_urdf::parseOrigin, str, tesseract_urdf::ORIGIN_ELEMENT_NAME.data()));
66  EXPECT_TRUE(origin.isApprox(check, 1e-6));
67  }
68 
69  {
70  std::string str = R"(<origin xyz="0 2.5 0" rpy="3.14159265359 0 0"/>)";
71  Eigen::Isometry3d origin;
72  EXPECT_TRUE(runTest<Eigen::Isometry3d>(
73  origin, &tesseract_urdf::parseOrigin, str, tesseract_urdf::ORIGIN_ELEMENT_NAME.data()));
74  EXPECT_TRUE(origin.translation().isApprox(Eigen::Vector3d(0, 2.5, 0), 1e-8));
75  EXPECT_TRUE(origin.matrix().col(0).head(3).isApprox(Eigen::Vector3d(1, 0, 0), 1e-8));
76  EXPECT_TRUE(origin.matrix().col(1).head(3).isApprox(Eigen::Vector3d(0, -1, 0), 1e-8));
77  EXPECT_TRUE(origin.matrix().col(2).head(3).isApprox(Eigen::Vector3d(0, 0, -1), 1e-8));
78 
79  Eigen::Quaterniond check_q = fromRPY(3.14159265359, 0, 0);
80  Eigen::Quaterniond orig_q(origin.rotation());
81 
82  EXPECT_TRUE(check_q.matrix().isApprox(orig_q.matrix(), 1e-8));
83  }
84 
85  {
86  std::string str = R"(<origin xyz="0 2.5 0" rpy="3.14 0 0" wxyz="1 0 0 0"/>)";
87  Eigen::Isometry3d origin;
88  EXPECT_TRUE(runTest<Eigen::Isometry3d>(
89  origin, &tesseract_urdf::parseOrigin, str, tesseract_urdf::ORIGIN_ELEMENT_NAME.data()));
90  EXPECT_TRUE(origin.translation().isApprox(Eigen::Vector3d(0, 2.5, 0), 1e-8));
91  EXPECT_TRUE(origin.rotation().isApprox(Eigen::Matrix3d::Identity(), 1e-8));
92  }
93 
94  {
95  std::string str = R"(<origin xyz="0 0 0"/>)";
96  Eigen::Isometry3d origin;
97  EXPECT_TRUE(runTest<Eigen::Isometry3d>(
98  origin, &tesseract_urdf::parseOrigin, str, tesseract_urdf::ORIGIN_ELEMENT_NAME.data()));
99  EXPECT_TRUE(origin.isApprox(Eigen::Isometry3d::Identity(), 1e-8));
100  }
101 
102  {
103  std::string str = R"(<origin rpy="0 0 0"/>)";
104  Eigen::Isometry3d origin;
105  EXPECT_TRUE(runTest<Eigen::Isometry3d>(
106  origin, &tesseract_urdf::parseOrigin, str, tesseract_urdf::ORIGIN_ELEMENT_NAME.data()));
107  EXPECT_TRUE(origin.isApprox(Eigen::Isometry3d::Identity(), 1e-8));
108  }
109 
110  {
111  std::string str = R"(<origin xyz="0 0 a"/>)";
112  Eigen::Isometry3d origin;
113  EXPECT_FALSE(runTest<Eigen::Isometry3d>(
114  origin, &tesseract_urdf::parseOrigin, str, tesseract_urdf::ORIGIN_ELEMENT_NAME.data()));
115  }
116 
117  {
118  std::string str = R"(<origin rpy="0 0 a"/>)";
119  Eigen::Isometry3d origin;
120  EXPECT_FALSE(runTest<Eigen::Isometry3d>(
121  origin, &tesseract_urdf::parseOrigin, str, tesseract_urdf::ORIGIN_ELEMENT_NAME.data()));
122  }
123 
124  {
125  std::string str = R"(<origin wxyz="1 0 0 a"/>)";
126  Eigen::Isometry3d origin;
127  EXPECT_FALSE(runTest<Eigen::Isometry3d>(
128  origin, &tesseract_urdf::parseOrigin, str, tesseract_urdf::ORIGIN_ELEMENT_NAME.data()));
129  }
130 
131  {
132  std::string str = R"(<origin />)";
133  Eigen::Isometry3d origin;
134  EXPECT_FALSE(runTest<Eigen::Isometry3d>(
135  origin, &tesseract_urdf::parseOrigin, str, tesseract_urdf::ORIGIN_ELEMENT_NAME.data()));
136  }
137 }
138 
139 TEST(TesseractURDFUnit, write_origin) // NOLINT
140 {
141  {
142  Eigen::Isometry3d origin = Eigen::Isometry3d::Identity();
143  std::string text;
144  EXPECT_EQ(0, writeTest<Eigen::Isometry3d>(origin, &tesseract_urdf::writeOrigin, text));
145  EXPECT_EQ(text, R"(<origin/>)");
146  }
147 
148  {
149  Eigen::Isometry3d origin = Eigen::Isometry3d::Identity();
150  origin.translation() = Eigen::Vector3d(1, 2, 3);
151  origin.linear() = Eigen::Quaterniond(1, 0, 0, 0).toRotationMatrix();
152  std::string text;
153  EXPECT_EQ(0, writeTest<Eigen::Isometry3d>(origin, &tesseract_urdf::writeOrigin, text));
154  EXPECT_EQ(text, R"(<origin xyz="1 2 3"/>)");
155  }
156 
157  {
158  Eigen::Isometry3d origin = Eigen::Isometry3d::Identity();
159  origin.translation() = Eigen::Vector3d(1, 2, 3);
160  origin.linear() = Eigen::Quaterniond(0, 0.577, 0.577, 0.577).toRotationMatrix();
161  std::string text;
162  EXPECT_EQ(0, writeTest<Eigen::Isometry3d>(origin, &tesseract_urdf::writeOrigin, text));
163  EXPECT_EQ(text, R"(<origin xyz="1 2 3" rpy="2.0359 -0.730089 2.03299"/>)");
164  }
165 }
tesseract_urdf::ORIGIN_ELEMENT_NAME
static constexpr std::string_view ORIGIN_ELEMENT_NAME
Definition: origin.h:43
TEST
TEST(TesseractURDFUnit, parse_origin)
Definition: tesseract_urdf_origin_unit.cpp:30
tesseract_urdf::parseOrigin
Eigen::Isometry3d parseOrigin(const tinyxml2::XMLElement *xml_element)
Parse xml element origin.
Definition: origin.cpp:42
origin.h
Parse origin from xml string.
TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
#define TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
EXPECT_TRUE
#define EXPECT_TRUE(args)
tesseract_urdf_common_unit.h
fromRPY
TESSERACT_COMMON_IGNORE_WARNINGS_PUSH TESSERACT_COMMON_IGNORE_WARNINGS_POP Eigen::Quaterniond fromRPY(double roll, double pitch, double yaw)
This function was pulled from urdfdom library to verify that the method of using Eigen produces the s...
Definition: tesseract_urdf_origin_unit.cpp:15
TESSERACT_COMMON_IGNORE_WARNINGS_POP
tesseract_urdf::writeOrigin
tinyxml2::XMLElement * writeOrigin(const Eigen::Isometry3d &origin, tinyxml2::XMLDocument &doc)
Definition: origin.cpp:129
macros.h
EXPECT_EQ
#define EXPECT_EQ(a, b)
EXPECT_FALSE
#define EXPECT_FALSE(args)

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