UrdfReaderTests.cpp
Go to the documentation of this file.
1 //
2 // Created by jordan on 5/18/17.
3 //
4 
5 #include <fstream>
6 #include <gtest/gtest.h>
7 #include <ros/package.h>
8 
9 #include "rdl_dynamics/Dynamics.h"
10 #include "rdl_dynamics/Kinematics.h"
11 #include "rdl_dynamics/Model.h"
12 #include "rdl_urdfreader/urdfreader.h"
13 
14 class UrdfReaderTests : public testing::Test
15 {
16  public:
17  UrdfReaderTests()
18  {
19  srand(time(nullptr));
20  }
21 
22  void SetUp()
23  {
24  }
25 
26  void TearDown()
27  {
28  }
29 
30  bool checkSpatialMatrixEpsilonClose(const RobotDynamics::Math::SpatialMatrix& t1, const RobotDynamics::Math::SpatialMatrix& t2, const double epsilon)
31  {
32  for (int i = 0; i < 6; i++)
33  {
34  for (int j = 0; j < 6; j++)
35  {
36  if (fabs(t1(i, j) - t2(i, j)) > epsilon)
37  {
38  return false;
39  }
40  }
41  }
42 
43  return true;
44  }
45 
46  bool checkSpatialVectorsEpsilonClose(const RobotDynamics::Math::SpatialVector& v1, const RobotDynamics::Math::SpatialVector& v2, const double epsilon)
47  {
48  for (int i = 0; i < 6; i++)
49  {
50  if (fabs(v1(i) - v2(i)) > epsilon)
51  {
52  return false;
53  }
54  }
55 
56  return true;
57  }
58 
59  std::string getFileContents(const std::string& filename)
60  {
61  std::ifstream model_file(filename);
62  if (!model_file)
63  {
64  std::cerr << "Error opening file '" << filename << "'." << std::endl;
65  abort();
66  }
67 
68  // reserve memory for the contents of the file
69  std::string model_xml_string;
70  model_file.seekg(0, std::ios::end);
71  model_xml_string.reserve(model_file.tellg());
72  model_file.seekg(0, std::ios::beg);
73  model_xml_string.assign((std::istreambuf_iterator<char>(model_file)), std::istreambuf_iterator<char>());
74 
75  model_file.close();
76  return model_xml_string;
77  }
78 };
79 
80 TEST_F(UrdfReaderTests, q_index_two_chains)
81 {
82  RobotDynamics::ModelPtr model(new RobotDynamics::Model());
83  std::string path_to_urdf = ros::package::getPath("rdl_urdfreader") + "/tests/urdf/simple_two_chain_bot.urdf";
84 
85  std::string contents = getFileContents(path_to_urdf);
86  std::vector<unsigned int> q_indices;
87  std::vector<std::string> joint_names, body_names;
88  EXPECT_TRUE(RobotDynamics::Urdf::parseJointAndBodyNamesFromString(contents, joint_names, body_names));
89  EXPECT_TRUE(RobotDynamics::Urdf::parseJointAndQIndex(contents, q_indices));
90 
91  EXPECT_EQ(q_indices.size(), 3);
92  EXPECT_STREQ(joint_names[0].c_str(), "test_robot_shoulder_lift_joint");
93  EXPECT_STREQ(body_names[0].c_str(), "test_robot_upper_arm_link");
94  EXPECT_EQ(q_indices[std::distance(joint_names.begin(), std::find(joint_names.begin(), joint_names.end(), "test_robot_shoulder_lift_joint"))], 0);
95 
96  EXPECT_STREQ(joint_names[1].c_str(), "test_robot_elbow_joint");
97  EXPECT_STREQ(body_names[1].c_str(), "test_robot_forearm_link");
98  EXPECT_EQ(q_indices[std::distance(joint_names.begin(), std::find(joint_names.begin(), joint_names.end(), "test_robot_elbow_joint"))], 1);
99 
100  EXPECT_STREQ(joint_names[2].c_str(), "test_robot_shoulder_pan_joint");
101  EXPECT_STREQ(body_names[2].c_str(), "test_robot_shoulder_link");
102  EXPECT_EQ(q_indices[std::distance(joint_names.begin(), std::find(joint_names.begin(), joint_names.end(), "test_robot_shoulder_pan_joint"))], 2);
103 }
104 
105 TEST_F(UrdfReaderTests, testFirstJointFixedNonTrivialTransform)
106 {
107  std::string file_path = ros::package::getPath("rdl_urdfreader") + "/tests/urdf/first_joint_fixed_non_trivial_xform.urdf";
108  RobotDynamics::ModelPtr model(new RobotDynamics::Model());
109 
110  EXPECT_TRUE(RobotDynamics::Urdf::urdfReadFromFile(file_path.c_str(), model, false, false));
111 
112  RobotDynamics::Math::VectorNd q = RobotDynamics::Math::VectorNd::Zero(model->q_size);
113  RobotDynamics::Math::VectorNd qdot = RobotDynamics::Math::VectorNd::Zero(model->qdot_size);
114  RobotDynamics::Math::VectorNd qddot = RobotDynamics::Math::VectorNd::Zero(model->qdot_size);
115 
116  ASSERT_EQ(model->q_size, 1);
117  ASSERT_EQ(model->qdot_size, 1);
118 
119  RobotDynamics::updateKinematics(*model, q, qdot, qddot);
120  RobotDynamics::ReferenceFramePtr frame = model->referenceFrameMap["j1_link"];
121 
122  EXPECT_TRUE(model->referenceFrameMap["j1_link"]->getInverseTransformToRoot().r.isApprox(RobotDynamics::Math::Vector3d(0.0, 0.16, 0.8377), 1e-4));
123 }
124 
125 TEST_F(UrdfReaderTests, testFixedArm)
126 {
127  std::string file_path = ros::package::getPath("rdl_urdfreader") + "/tests/urdf/test_robot_arm.urdf";
128  RobotDynamics::ModelPtr model(new RobotDynamics::Model());
129  EXPECT_TRUE(RobotDynamics::Urdf::urdfReadFromFile(file_path.c_str(), model, false, false));
130 
131  // First body in URDf is a fixed joint body, so it'll get mashed together with the root body
132  EXPECT_EQ(model->mBodies[0].mMass, 4.);
133  EXPECT_TRUE(model->mBodies[0].mCenterOfMass.isApprox(RobotDynamics::Math::Vector3d(0., 0., 0.), 1e-14));
134 
135  EXPECT_EQ(model->Ib_c[0].Ixx, 0.0061063308908);
136  EXPECT_EQ(model->Ib_c[0].Iyx, 0.0);
137  EXPECT_EQ(model->Ib_c[0].Izx, 0.0);
138  EXPECT_EQ(model->Ib_c[0].Iyy, 0.0061063308908);
139  EXPECT_EQ(model->Ib_c[0].Izy, 0.0);
140  EXPECT_EQ(model->Ib_c[0].Izz, 0.01125);
141 
142  unsigned int id = model->GetBodyId("test_robot_shoulder_link");
143  EXPECT_EQ(model->mBodies[id].mMass, 7.778);
144  EXPECT_TRUE(model->mBodies[id].mCenterOfMass.isApprox(RobotDynamics::Math::Vector3d(0., 0.01, 0.), 1e-14));
145 
146  EXPECT_EQ(model->Ib_c[id].Ixx, 0.0314743125769);
147  EXPECT_EQ(model->Ib_c[id].Iyx, 0.);
148  EXPECT_EQ(model->Ib_c[id].Izx, 0.);
149  EXPECT_EQ(model->Ib_c[id].Iyy, 0.0314743125769);
150  EXPECT_EQ(model->Ib_c[id].Izy, 0.);
151  EXPECT_EQ(model->Ib_c[id].Izz, 0.021875625);
152 
153  id = model->GetParentBodyId(model->GetBodyId("gripper_right_finger_link"));
154  EXPECT_EQ(model->GetBodyId("gripper_right_knuckle_link"), id);
155 
156  EXPECT_EQ(model->mJoints[1].mJointType, RobotDynamics::JointTypePrismatic);
157  EXPECT_EQ(model->GetBodyId("test_robot_shoulder_link"), model->lambda[model->GetBodyId("test_robot_upper_arm_link")]);
158 }
159 
160 TEST_F(UrdfReaderTests, testNegative1DofJointAxes)
161 {
162  std::string file_path = ros::package::getPath("rdl_urdfreader") + "/tests/urdf/test_robot_arm.urdf";
163  std::string file_path_2 = ros::package::getPath("rdl_urdfreader") + "/tests/urdf/test_robot_arm_neg_jnt_axes.urdf";
164  RobotDynamics::ModelPtr model(new RobotDynamics::Model()), model_neg_axes(new RobotDynamics::Model());
165  EXPECT_TRUE(RobotDynamics::Urdf::urdfReadFromFile(file_path.c_str(), model, false, false));
166  EXPECT_TRUE(RobotDynamics::Urdf::urdfReadFromFile(file_path_2.c_str(), model_neg_axes, false, false));
167 
168  RobotDynamics::Math::VectorNd q = RobotDynamics::Math::VectorNd::Zero(model->q_size);
169  RobotDynamics::Math::VectorNd q_neg = RobotDynamics::Math::VectorNd::Zero(model->q_size);
170  RobotDynamics::Math::VectorNd qd = RobotDynamics::Math::VectorNd::Zero(model->qdot_size);
171  RobotDynamics::Math::VectorNd qd_neg = RobotDynamics::Math::VectorNd::Zero(model->qdot_size);
172  RobotDynamics::Math::VectorNd qdd = RobotDynamics::Math::VectorNd::Zero(model->qdot_size);
173  RobotDynamics::Math::VectorNd qdd_neg = RobotDynamics::Math::VectorNd::Zero(model->qdot_size);
174  RobotDynamics::Math::VectorNd tau = RobotDynamics::Math::VectorNd::Zero(model->qdot_size);
175  RobotDynamics::Math::VectorNd tau_neg = RobotDynamics::Math::VectorNd::Zero(model->qdot_size);
176 
177  for (int i = 0; i < q.size(); i++)
178  {
179  q[i] = 0.4 * M_PI * static_cast<double>(rand()) / static_cast<double>(RAND_MAX);
180  qd[i] = 0.5 * M_PI * static_cast<double>(rand()) / static_cast<double>(RAND_MAX);
181  tau[i] = 0.5 * M_PI * static_cast<double>(rand()) / static_cast<double>(RAND_MAX);
182  qdd[i] = 0.5 * M_PI * static_cast<double>(rand()) / static_cast<double>(RAND_MAX);
183  }
184 
185  q_neg = -q;
186  qd_neg = -qd;
187  qdd_neg = -qdd;
188  tau_neg = -tau;
189 
190  RobotDynamics::updateKinematics(*model, q, qd, qdd);
191  RobotDynamics::updateKinematics(*model_neg_axes, q_neg, qd_neg, qdd_neg);
192 
193  for (unsigned int i = 0; i < model->mBodies.size(); i++)
194  {
195  EXPECT_TRUE(checkSpatialVectorsEpsilonClose(model->v[i], model_neg_axes->v[i], 1e-14));
196  EXPECT_TRUE(checkSpatialVectorsEpsilonClose(model->a[i], model_neg_axes->a[i], 1e-14));
197  }
198 
199  RobotDynamics::forwardDynamics(*model, q, qd, tau, qdd);
200  RobotDynamics::forwardDynamics(*model_neg_axes, q_neg, qd_neg, tau_neg, qdd_neg);
201 
202  EXPECT_TRUE(qdd.isApprox(-qdd_neg, 1e-14));
203 
204  RobotDynamics::inverseDynamics(*model, q, qd, qdd, tau);
205  RobotDynamics::inverseDynamics(*model_neg_axes, q_neg, qd_neg, qdd_neg, tau_neg);
206 
207  EXPECT_TRUE(tau.isApprox(-tau_neg, 1e-14));
208 }
209 
210 TEST_F(UrdfReaderTests, testJointBodyMap)
211 {
212  std::string file_path = ros::package::getPath("rdl_urdfreader") + "/tests/urdf/test_robot_arm.urdf";
213  std::map<std::string, std::string> jointBodyMap;
214 
215  ASSERT_TRUE(RobotDynamics::Urdf::parseJointBodyNameMapFromFile(file_path.c_str(), jointBodyMap));
216 
217  EXPECT_STREQ(jointBodyMap["test_robot_elbow_joint"].c_str(), "test_robot_forearm_link");
218  EXPECT_TRUE(jointBodyMap.find("test_robot_ee_fixed_joint") == jointBodyMap.end()); // It's fixed, so shouldn't be here
219  EXPECT_STREQ(jointBodyMap["test_robot_shoulder_pan_joint"].c_str(), "test_robot_shoulder_link");
220 
221  std::string contents = getFileContents(file_path);
222  std::vector<unsigned int> q_indices;
223  RobotDynamics::Urdf::parseJointAndQIndex(contents, q_indices);
224  EXPECT_EQ(q_indices.size(), 12);
225  // EXPECT_TRUE(q_indices[0] == 0);
226 }
227 
228 TEST_F(UrdfReaderTests, testFloatingBaseRobot)
229 {
230  RobotDynamics::ModelPtr model(new RobotDynamics::Model());
231  std::string path_to_urdf = ros::package::getPath("rdl_urdfreader") + "/tests/urdf/floating_base_robot.urdf";
232  EXPECT_TRUE(RobotDynamics::Urdf::urdfReadFromFile(path_to_urdf.c_str(), model, true, false));
233 
234  EXPECT_NEAR(model->mBodies[2].mMass, 4., 1e-14);
235  EXPECT_TRUE(model->mBodies[2].mCenterOfMass.isApprox(RobotDynamics::Math::Vector3d(0.3, 0.2, 0.1), 1e-14));
236 
237  EXPECT_NEAR(model->Ib_c[2].Ixx, 0.1, 1e-14);
238  EXPECT_NEAR(model->Ib_c[2].Iyx, 0., 1e-14);
239  EXPECT_NEAR(model->Ib_c[2].Izx, 0., 1e-14);
240  EXPECT_NEAR(model->Ib_c[2].Iyy, 0.2, 1e-14);
241  EXPECT_NEAR(model->Ib_c[2].Izy, 0., 1e-14);
242  EXPECT_NEAR(model->Ib_c[2].Izz, 0.3, 1e-14);
243 
244  EXPECT_EQ(model->mJoints[1].mJointType, RobotDynamics::JointTypeTranslationXYZ);
245  EXPECT_EQ(model->mJoints[2].mJointType, RobotDynamics::JointTypeSpherical);
246 }
247 
248 TEST_F(UrdfReaderTests, testFloatingBaseRobotDeduceFloatingBase)
249 {
250  RobotDynamics::ModelPtr model(new RobotDynamics::Model());
251  std::string path_to_urdf = ros::package::getPath("rdl_urdfreader") + "/tests/urdf/floating_base_robot.urdf";
252  EXPECT_TRUE(RobotDynamics::Urdf::urdfReadFromFile(path_to_urdf, model));
253 
254  EXPECT_NEAR(model->mBodies[2].mMass, 4., 1e-14);
255  EXPECT_TRUE(model->mBodies[2].mCenterOfMass.isApprox(RobotDynamics::Math::Vector3d(0.3, 0.2, 0.1), 1e-14));
256 
257  EXPECT_NEAR(model->Ib_c[2].Ixx, 0.1, 1e-14);
258  EXPECT_NEAR(model->Ib_c[2].Iyx, 0., 1e-14);
259  EXPECT_NEAR(model->Ib_c[2].Izx, 0., 1e-14);
260  EXPECT_NEAR(model->Ib_c[2].Iyy, 0.2, 1e-14);
261  EXPECT_NEAR(model->Ib_c[2].Izy, 0., 1e-14);
262  EXPECT_NEAR(model->Ib_c[2].Izz, 0.3, 1e-14);
263 
264  EXPECT_EQ(model->mJoints[1].mJointType, RobotDynamics::JointTypeTranslationXYZ);
265  EXPECT_EQ(model->mJoints[2].mJointType, RobotDynamics::JointTypeSpherical);
266 }
267 
268 TEST_F(UrdfReaderTests, testRobotWithFloatingJoint)
269 {
270  RobotDynamics::ModelPtr model(new RobotDynamics::Model());
271  std::string path_to_urdf = ros::package::getPath("rdl_urdfreader") + "/tests/urdf/floating_joint_robot.urdf";
272  EXPECT_TRUE(RobotDynamics::Urdf::urdfReadFromFile(path_to_urdf.c_str(), model, true, false));
273 
274  EXPECT_EQ(model->mJoints[1].mJointType, RobotDynamics::JointTypeTranslationXYZ);
275  EXPECT_EQ(model->mJoints[2].mJointType, RobotDynamics::JointTypeEulerXYZ);
276 
277  std::string contents = getFileContents(path_to_urdf);
278  std::vector<unsigned int> q_indices;
279  EXPECT_TRUE(RobotDynamics::Urdf::parseJointAndQIndex(contents, q_indices));
280 
281  EXPECT_EQ(q_indices.size(), 1);
282  EXPECT_EQ(q_indices[0], 6);
283 }
284 
285 TEST_F(UrdfReaderTests, testRobotSingleBodyFloatingBase)
286 {
287  RobotDynamics::ModelPtr model(new RobotDynamics::Model());
288  std::string path_to_urdf = ros::package::getPath("rdl_urdfreader") + "/tests/urdf/single_body_floating_base.urdf";
289  ASSERT_TRUE(RobotDynamics::Urdf::urdfReadFromFile(path_to_urdf.c_str(), model, true, false));
290 
291  std::string contents = getFileContents(path_to_urdf);
292  std::vector<unsigned int> q_indices;
293  EXPECT_TRUE(RobotDynamics::Urdf::parseJointAndQIndex(contents, q_indices));
294 
295  EXPECT_EQ(q_indices.size(), 0);
296 }
297 
298 TEST_F(UrdfReaderTests, rotated_inertial)
299 {
300  RobotDynamics::ModelPtr model(new RobotDynamics::Model());
301  std::string path_to_urdf = ros::package::getPath("rdl_urdfreader") + "/tests/urdf/rotated_inertial.urdf";
302  ASSERT_TRUE(RobotDynamics::Urdf::urdfReadFromFile(path_to_urdf.c_str(), model, true, false));
303 
304  RobotDynamics::Math::Matrix3d I1 = RobotDynamics::Math::Matrix3dIdentity;
305  I1(0, 0) = 0.03;
306  I1(1, 1) = 0.03;
307  I1(2, 2) = 0.02;
308  EXPECT_TRUE(I1.isApprox(model->mBodies[1].mInertia, 1.e-10));
309 
310  RobotDynamics::Math::Matrix3d I2 = RobotDynamics::Math::Matrix3dIdentity;
311  I2(0, 0) = 0.03;
312  I2(1, 1) = 0.02;
313  I2(2, 2) = 0.03;
314  EXPECT_TRUE(I2.isApprox(model->mBodies[2].mInertia, 1.e-10));
315 
316  RobotDynamics::Math::Matrix3d I3 = RobotDynamics::Math::Matrix3dIdentity;
317  I3(0, 0) = 0.02;
318  I3(1, 1) = 0.03;
319  I3(2, 2) = 0.03;
320  EXPECT_TRUE(I3.isApprox(model->mBodies[3].mInertia, 1.e-10));
321 }
322 
323 TEST_F(UrdfReaderTests, rotated_inertial_floating_base)
324 {
325  RobotDynamics::ModelPtr model(new RobotDynamics::Model());
326  std::string path_to_urdf = ros::package::getPath("rdl_urdfreader") + "/tests/urdf/floating_base_rotated_inertial.urdf";
327  ASSERT_TRUE(RobotDynamics::Urdf::urdfReadFromFile(path_to_urdf.c_str(), model, true, false));
328 
329  RobotDynamics::Math::Matrix3d I1 = RobotDynamics::Math::Matrix3dIdentity;
330  I1(0, 0) = 0.03;
331  I1(1, 1) = 0.03;
332  I1(2, 2) = 0.02;
333  EXPECT_TRUE(I1.isApprox(model->mBodies[2].mInertia, 1.e-10));
334 
335  RobotDynamics::Math::Matrix3d I2 = RobotDynamics::Math::Matrix3dIdentity;
336  I2(0, 0) = 0.03;
337  I2(1, 1) = 0.02;
338  I2(2, 2) = 0.03;
339  EXPECT_TRUE(I2.isApprox(model->mBodies[3].mInertia, 1.e-10));
340 
341  RobotDynamics::Math::Matrix3d I3 = RobotDynamics::Math::Matrix3dIdentity;
342  I3(0, 0) = 0.02;
343  I3(1, 1) = 0.03;
344  I3(2, 2) = 0.03;
345  EXPECT_TRUE(I3.isApprox(model->mBodies[4].mInertia, 1.e-10));
346 }
347 
348 int main(int argc, char** argv)
349 {
350  ::testing::InitGoogleTest(&argc, argv);
351  ::testing::FLAGS_gtest_death_test_style = "threadsafe";
352  return RUN_ALL_TESTS();
353 }
UrdfReaderTests::checkSpatialMatrixEpsilonClose
bool checkSpatialMatrixEpsilonClose(const RobotDynamics::Math::SpatialMatrix &t1, const RobotDynamics::Math::SpatialMatrix &t2, const double epsilon)
Definition: UrdfReaderTests.cpp:30
RobotDynamics::Math::Matrix3dIdentity
Matrix3d Matrix3dIdentity(1., 0., 0., 0., 1., 0., 0., 0., 1)
RobotDynamics::ModelPtr
std::shared_ptr< Model > ModelPtr
RobotDynamics::Urdf::parseJointBodyNameMapFromFile
bool parseJointBodyNameMapFromFile(const char *filename, std::map< std::string, std::string > &jointBodyMap)
This will build a map of joint name to body name.
Definition: urdfreader.cc:274
main
int main(int argc, char **argv)
Definition: UrdfReaderTests.cpp:348
RobotDynamics::Urdf::parseJointAndBodyNamesFromString
bool parseJointAndBodyNamesFromString(const std::string &model_xml_string, std::vector< std::string > &joint_names, std::vector< std::string > &body_names)
This will build vectors of joint name and body name pairs.
Definition: urdfreader.cc:323
TEST_F
TEST_F(UrdfReaderTests, q_index_two_chains)
Definition: UrdfReaderTests.cpp:80
RobotDynamics::ReferenceFramePtr
std::shared_ptr< ReferenceFrame > ReferenceFramePtr
RobotDynamics::Math::VectorNd
Eigen::VectorXd VectorNd
RobotDynamics::updateKinematics
void updateKinematics(Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, const Math::VectorNd &QDDot)
UrdfReaderTests::getFileContents
std::string getFileContents(const std::string &filename)
Definition: UrdfReaderTests.cpp:59
filename
string filename
Definition: rdl_urdfreader_util.cc:11
ros::package::getPath
ROSLIB_DECL std::string getPath(const std::string &package_name)
UrdfReaderTests::checkSpatialVectorsEpsilonClose
bool checkSpatialVectorsEpsilonClose(const RobotDynamics::Math::SpatialVector &v1, const RobotDynamics::Math::SpatialVector &v2, const double epsilon)
Definition: UrdfReaderTests.cpp:46
RobotDynamics::Urdf::urdfReadFromFile
bool urdfReadFromFile(const char *filename, ModelPtr model, bool floating_base, bool verbose=false)
Read urdf from file path.
Definition: urdfreader.cc:390
RobotDynamics::inverseDynamics
void inverseDynamics(Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, const Math::VectorNd &QDDot, Math::VectorNd &Tau, Math::SpatialForceV *f_ext=nullptr, bool update_kinematics=true)
RobotDynamics::forwardDynamics
void forwardDynamics(Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, const Math::VectorNd &Tau, Math::VectorNd &QDDot, Math::SpatialForceV *f_ext=nullptr, bool update_kinematics=true)
RobotDynamics::Urdf::parseJointAndQIndex
bool parseJointAndQIndex(const RobotDynamics::Model &model, const std::vector< std::string > &body_names, std::vector< unsigned int > &q_indices)
This will build a vector of joint indices in the same order as the list of joints.
Definition: urdfreader.cc:362

rdl_urdfreader
Author(s):
autogenerated on Tue Apr 20 2021 02:25:38