Branch data Line data Source code
1 : : // Copyright (c) 2017 Jordan Lack <jlack1987@gmail.com>
2 : : // Copyright (c) 2011-2016 Martin Felis <martin.felis@iwr.uni-heidelberg.de>
3 : : // RDL - Robot Dynamics Library
4 : : // Licensed under the zlib license. See LICENSE for more details.
5 : :
6 : : #include <fstream>
7 : : #include <gtest/gtest.h>
8 : : #include <ament_index_cpp/get_package_share_directory.hpp>
9 : :
10 : : #include <rdl_dynamics/Dynamics.hpp>
11 : : #include <rdl_dynamics/Kinematics.hpp>
12 : : #include <rdl_dynamics/Model.hpp>
13 : : #include <rdl_dynamics/rdl_utils.hpp>
14 : : #include <rdl_urdfreader/urdfreader.hpp>
15 : :
16 : : const double E_MINUS_14 = 1.0e-14;
17 : :
18 : : class UrdfReaderTests : public testing::Test
19 : : {
20 : : public:
21 : 18 : UrdfReaderTests()
22 : 18 : {
23 : 18 : srand(time(nullptr));
24 : 18 : }
25 : :
26 : 18 : void SetUp()
27 : : {
28 : 18 : }
29 : :
30 : 18 : void TearDown()
31 : : {
32 : 18 : }
33 : :
34 : 8 : std::string getFileContents(const std::string& filename)
35 : : {
36 : 8 : std::ifstream model_file(filename);
37 : 8 : if (!model_file)
38 : : {
39 : 0 : std::cerr << "Error opening file '" << filename << "'." << std::endl;
40 : 0 : abort();
41 : : }
42 : :
43 : : // reserve memory for the contents of the file
44 : 8 : std::string model_xml_string;
45 : 8 : model_file.seekg(0, std::ios::end);
46 : 8 : model_xml_string.reserve(model_file.tellg());
47 : 8 : model_file.seekg(0, std::ios::beg);
48 : 8 : model_xml_string.assign((std::istreambuf_iterator<char>(model_file)), std::istreambuf_iterator<char>());
49 : :
50 : 8 : model_file.close();
51 : 16 : return model_xml_string;
52 : 8 : }
53 : : };
54 : :
55 : 2 : TEST_F(UrdfReaderTests, q_index_two_chains)
56 : : {
57 : 1 : RobotDynamics::ModelPtr model(new RobotDynamics::Model());
58 : 2 : std::string path_to_urdf = ament_index_cpp::get_package_share_directory("rdl_urdfreader") + "/tests/urdf/simple_two_chain_bot.urdf";
59 : :
60 : 1 : std::string contents = getFileContents(path_to_urdf);
61 : 1 : std::vector<unsigned int> q_indices;
62 : 1 : std::vector<std::string> joint_names, body_names;
63 : 1 : EXPECT_TRUE(RobotDynamics::Urdf::parseJointAndBodyNamesFromString(contents, joint_names, body_names));
64 : 1 : EXPECT_TRUE(RobotDynamics::Urdf::parseJointAndQIndex(contents, q_indices));
65 : :
66 : 1 : EXPECT_EQ(q_indices.size(), (unsigned int)3);
67 : 1 : EXPECT_STREQ(joint_names[0].c_str(), "test_robot_shoulder_lift_joint");
68 : 1 : EXPECT_STREQ(body_names[0].c_str(), "test_robot_upper_arm_link");
69 : 1 : EXPECT_EQ(q_indices[std::distance(joint_names.begin(), std::find(joint_names.begin(), joint_names.end(), "test_robot_shoulder_lift_joint"))], (unsigned int)0);
70 : :
71 : 1 : EXPECT_STREQ(joint_names[1].c_str(), "test_robot_elbow_joint");
72 : 1 : EXPECT_STREQ(body_names[1].c_str(), "test_robot_forearm_link");
73 : 1 : EXPECT_EQ(q_indices[std::distance(joint_names.begin(), std::find(joint_names.begin(), joint_names.end(), "test_robot_elbow_joint"))], (unsigned int)1);
74 : :
75 : 1 : EXPECT_STREQ(joint_names[2].c_str(), "test_robot_shoulder_pan_joint");
76 : 1 : EXPECT_STREQ(body_names[2].c_str(), "test_robot_shoulder_link");
77 : 1 : EXPECT_EQ(q_indices[std::distance(joint_names.begin(), std::find(joint_names.begin(), joint_names.end(), "test_robot_shoulder_pan_joint"))], (unsigned int)2);
78 : 1 : }
79 : :
80 : 2 : TEST_F(UrdfReaderTests, testFirstJointFixedNonTrivialTransform)
81 : : {
82 : 2 : std::string file_path = ament_index_cpp::get_package_share_directory("rdl_urdfreader") + "/tests/urdf/first_joint_fixed_non_trivial_xform.urdf";
83 : 1 : RobotDynamics::ModelPtr model(new RobotDynamics::Model());
84 : :
85 : 1 : EXPECT_TRUE(RobotDynamics::Urdf::urdfReadFromFile(file_path, model));
86 : :
87 : 1 : RobotDynamics::Math::VectorNd q = RobotDynamics::Math::VectorNd::Zero(model->q_size);
88 : 1 : RobotDynamics::Math::VectorNd qdot = RobotDynamics::Math::VectorNd::Zero(model->qdot_size);
89 : 1 : RobotDynamics::Math::VectorNd qddot = RobotDynamics::Math::VectorNd::Zero(model->qdot_size);
90 : :
91 : 1 : ASSERT_EQ(model->q_size, (unsigned int)1);
92 : 1 : ASSERT_EQ(model->qdot_size, (unsigned int)1);
93 : :
94 : 1 : RobotDynamics::updateKinematics(*model, q, qdot, qddot);
95 : 3 : RobotDynamics::ReferenceFramePtr frame = model->referenceFrameMap["j1_link"];
96 : :
97 : 1 : EXPECT_TRUE(model->referenceFrameMap["j1_link"]->getInverseTransformToRoot().r.isApprox(RobotDynamics::Math::Vector3d(0.0, 0.16, 0.8377), 1e-4));
98 : 1 : }
99 : :
100 : 2 : TEST_F(UrdfReaderTests, testFixedArm)
101 : : {
102 : 2 : std::string file_path = ament_index_cpp::get_package_share_directory("rdl_urdfreader") + "/tests/urdf/test_robot_arm.urdf";
103 : 1 : RobotDynamics::ModelPtr model(new RobotDynamics::Model());
104 : 1 : EXPECT_TRUE(RobotDynamics::Urdf::urdfReadFromFile(file_path, model));
105 : :
106 : : // First body in URDf is a fixed joint body, so it'll get mashed together with the root body
107 : 1 : EXPECT_EQ(model->mBodies[0].mMass, 4.);
108 : 1 : EXPECT_TRUE(model->mBodies[0].mCenterOfMass.isApprox(RobotDynamics::Math::Vector3d(0., 0., 0.), 1e-14));
109 : :
110 : 1 : EXPECT_EQ(model->Ib_c[0].Ixx, 0.0061063308908);
111 : 1 : EXPECT_EQ(model->Ib_c[0].Iyx, 0.0);
112 : 1 : EXPECT_EQ(model->Ib_c[0].Izx, 0.0);
113 : 1 : EXPECT_EQ(model->Ib_c[0].Iyy, 0.0061063308908);
114 : 1 : EXPECT_EQ(model->Ib_c[0].Izy, 0.0);
115 : 1 : EXPECT_EQ(model->Ib_c[0].Izz, 0.01125);
116 : :
117 : 1 : unsigned int id = model->GetBodyId("test_robot_shoulder_link");
118 : 1 : EXPECT_EQ(model->mBodies[id].mMass, 7.778);
119 : 1 : EXPECT_TRUE(model->mBodies[id].mCenterOfMass.isApprox(RobotDynamics::Math::Vector3d(0., 0.01, 0.), 1e-14));
120 : :
121 : 1 : EXPECT_EQ(model->Ib_c[id].Ixx, 0.0314743125769);
122 : 1 : EXPECT_EQ(model->Ib_c[id].Iyx, 0.);
123 : 1 : EXPECT_EQ(model->Ib_c[id].Izx, 0.);
124 : 1 : EXPECT_EQ(model->Ib_c[id].Iyy, 0.0314743125769);
125 : 1 : EXPECT_EQ(model->Ib_c[id].Izy, 0.);
126 : 1 : EXPECT_EQ(model->Ib_c[id].Izz, 0.021875625);
127 : :
128 : 1 : id = model->GetParentBodyId(model->GetBodyId("gripper_right_finger_link"));
129 : 1 : EXPECT_EQ(model->GetBodyId("gripper_right_knuckle_link"), id);
130 : :
131 : 1 : EXPECT_EQ(model->mJoints[1].mJointType, RobotDynamics::JointTypePrismatic);
132 : 1 : EXPECT_EQ(model->GetBodyId("test_robot_shoulder_link"), model->lambda[model->GetBodyId("test_robot_upper_arm_link")]);
133 : 1 : }
134 : :
135 : 2 : TEST_F(UrdfReaderTests, testNegative1DofJointAxes)
136 : : {
137 : 2 : std::string file_path = ament_index_cpp::get_package_share_directory("rdl_urdfreader") + "/tests/urdf/test_robot_arm.urdf";
138 : 2 : std::string file_path_2 = ament_index_cpp::get_package_share_directory("rdl_urdfreader") + "/tests/urdf/test_robot_arm_neg_jnt_axes.urdf";
139 : 2 : RobotDynamics::ModelPtr model(new RobotDynamics::Model()), model_neg_axes(new RobotDynamics::Model());
140 : 1 : EXPECT_TRUE(RobotDynamics::Urdf::urdfReadFromFile(file_path, model));
141 : 1 : EXPECT_TRUE(RobotDynamics::Urdf::urdfReadFromFile(file_path_2, model_neg_axes));
142 : :
143 : 2 : RobotDynamics::Math::VectorNd q = RobotDynamics::Math::VectorNd::Zero(model->q_size);
144 : 2 : RobotDynamics::Math::VectorNd q_neg = RobotDynamics::Math::VectorNd::Zero(model->q_size);
145 : 2 : RobotDynamics::Math::VectorNd qd = RobotDynamics::Math::VectorNd::Zero(model->qdot_size);
146 : 2 : RobotDynamics::Math::VectorNd qd_neg = RobotDynamics::Math::VectorNd::Zero(model->qdot_size);
147 : 2 : RobotDynamics::Math::VectorNd qdd = RobotDynamics::Math::VectorNd::Zero(model->qdot_size);
148 : 2 : RobotDynamics::Math::VectorNd qdd_neg = RobotDynamics::Math::VectorNd::Zero(model->qdot_size);
149 : 2 : RobotDynamics::Math::VectorNd tau = RobotDynamics::Math::VectorNd::Zero(model->qdot_size);
150 : 2 : RobotDynamics::Math::VectorNd tau_neg = RobotDynamics::Math::VectorNd::Zero(model->qdot_size);
151 : :
152 : 13 : for (int i = 0; i < q.size(); i++)
153 : : {
154 : 12 : q[i] = 0.4 * M_PI * static_cast<double>(rand()) / static_cast<double>(RAND_MAX);
155 : 12 : qd[i] = 0.5 * M_PI * static_cast<double>(rand()) / static_cast<double>(RAND_MAX);
156 : 12 : tau[i] = 0.5 * M_PI * static_cast<double>(rand()) / static_cast<double>(RAND_MAX);
157 : 12 : qdd[i] = 0.5 * M_PI * static_cast<double>(rand()) / static_cast<double>(RAND_MAX);
158 : : }
159 : :
160 : 1 : q_neg = -q;
161 : 1 : qd_neg = -qd;
162 : 1 : qdd_neg = -qdd;
163 : 1 : tau_neg = -tau;
164 : :
165 : 1 : RobotDynamics::updateKinematics(*model, q, qd, qdd);
166 : 1 : RobotDynamics::updateKinematics(*model_neg_axes, q_neg, qd_neg, qdd_neg);
167 : :
168 : 14 : for (unsigned int i = 0; i < model->mBodies.size(); i++)
169 : : {
170 : 13 : EXPECT_TRUE(model->v[i].isApprox(model_neg_axes->v[i], 1e-14));
171 : 13 : EXPECT_TRUE(model->a[i].isApprox(model_neg_axes->a[i], 1e-14));
172 : : }
173 : :
174 : 1 : RobotDynamics::forwardDynamics(*model, q, qd, tau, qdd);
175 : 1 : RobotDynamics::forwardDynamics(*model_neg_axes, q_neg, qd_neg, tau_neg, qdd_neg);
176 : :
177 : 1 : EXPECT_TRUE(qdd.isApprox(-qdd_neg, 1e-14));
178 : :
179 : 1 : RobotDynamics::inverseDynamics(*model, q, qd, qdd, tau);
180 : 1 : RobotDynamics::inverseDynamics(*model_neg_axes, q_neg, qd_neg, qdd_neg, tau_neg);
181 : :
182 : 1 : EXPECT_TRUE(tau.isApprox(-tau_neg, 1e-14));
183 : 1 : }
184 : :
185 : 2 : TEST_F(UrdfReaderTests, testJointBodyMap)
186 : : {
187 : 2 : std::string file_path = ament_index_cpp::get_package_share_directory("rdl_urdfreader") + "/tests/urdf/test_robot_arm.urdf";
188 : 1 : std::map<std::string, std::string> jointBodyMap;
189 : :
190 : 1 : ASSERT_TRUE(RobotDynamics::Urdf::parseJointBodyNameMapFromFile(file_path, jointBodyMap));
191 : :
192 : 2 : EXPECT_STREQ(jointBodyMap["test_robot_elbow_joint"].c_str(), "test_robot_forearm_link");
193 : 1 : EXPECT_TRUE(jointBodyMap.find("test_robot_ee_fixed_joint") == jointBodyMap.end()); // It's fixed, so shouldn't be here
194 : 2 : EXPECT_STREQ(jointBodyMap["test_robot_shoulder_pan_joint"].c_str(), "test_robot_shoulder_link");
195 : :
196 : 2 : std::string contents = getFileContents(file_path);
197 : 2 : std::vector<unsigned int> q_indices;
198 : 1 : RobotDynamics::Urdf::parseJointAndQIndex(contents, q_indices);
199 : 1 : EXPECT_EQ(q_indices.size(), (unsigned int)12);
200 : : // EXPECT_TRUE(q_indices[0] == 0);
201 : 1 : }
202 : :
203 : 2 : TEST_F(UrdfReaderTests, testFloatingBaseRobot)
204 : : {
205 : 1 : RobotDynamics::ModelPtr model(new RobotDynamics::Model());
206 : 2 : std::string path_to_urdf = ament_index_cpp::get_package_share_directory("rdl_urdfreader") + "/tests/urdf/floating_base_robot.urdf";
207 : 1 : EXPECT_TRUE(RobotDynamics::Urdf::urdfReadFromFile(path_to_urdf, model));
208 : :
209 : 1 : EXPECT_NEAR(model->mBodies[2].mMass, 4., 1e-14);
210 : 1 : EXPECT_TRUE(model->mBodies[2].mCenterOfMass.isApprox(RobotDynamics::Math::Vector3d(0.3, 0.2, 0.1), 1e-14));
211 : :
212 : 1 : EXPECT_NEAR(model->Ib_c[2].Ixx, 0.1, 1e-14);
213 : 1 : EXPECT_NEAR(model->Ib_c[2].Iyx, 0., 1e-14);
214 : 1 : EXPECT_NEAR(model->Ib_c[2].Izx, 0., 1e-14);
215 : 1 : EXPECT_NEAR(model->Ib_c[2].Iyy, 0.2, 1e-14);
216 : 1 : EXPECT_NEAR(model->Ib_c[2].Izy, 0., 1e-14);
217 : 1 : EXPECT_NEAR(model->Ib_c[2].Izz, 0.3, 1e-14);
218 : :
219 : 1 : EXPECT_EQ(model->mJoints[1].mJointType, RobotDynamics::JointTypeTranslationXYZ);
220 : 1 : EXPECT_EQ(model->mJoints[2].mJointType, RobotDynamics::JointTypeSpherical);
221 : 1 : }
222 : :
223 : 2 : TEST_F(UrdfReaderTests, testFloatingBaseRobotDeduceFloatingBase)
224 : : {
225 : 1 : RobotDynamics::ModelPtr model(new RobotDynamics::Model());
226 : 2 : std::string path_to_urdf = ament_index_cpp::get_package_share_directory("rdl_urdfreader") + "/tests/urdf/floating_base_robot.urdf";
227 : 1 : EXPECT_TRUE(RobotDynamics::Urdf::urdfReadFromFile(path_to_urdf, model));
228 : :
229 : 1 : EXPECT_NEAR(model->mBodies[2].mMass, 4., 1e-14);
230 : 1 : EXPECT_TRUE(model->mBodies[2].mCenterOfMass.isApprox(RobotDynamics::Math::Vector3d(0.3, 0.2, 0.1), 1e-14));
231 : :
232 : 1 : EXPECT_NEAR(model->Ib_c[2].Ixx, 0.1, 1e-14);
233 : 1 : EXPECT_NEAR(model->Ib_c[2].Iyx, 0., 1e-14);
234 : 1 : EXPECT_NEAR(model->Ib_c[2].Izx, 0., 1e-14);
235 : 1 : EXPECT_NEAR(model->Ib_c[2].Iyy, 0.2, 1e-14);
236 : 1 : EXPECT_NEAR(model->Ib_c[2].Izy, 0., 1e-14);
237 : 1 : EXPECT_NEAR(model->Ib_c[2].Izz, 0.3, 1e-14);
238 : :
239 : 1 : EXPECT_EQ(model->mJoints[1].mJointType, RobotDynamics::JointTypeTranslationXYZ);
240 : 1 : EXPECT_EQ(model->mJoints[2].mJointType, RobotDynamics::JointTypeSpherical);
241 : 1 : }
242 : :
243 : 2 : TEST_F(UrdfReaderTests, testRobotWithFloatingJoint)
244 : : {
245 : 1 : RobotDynamics::ModelPtr model(new RobotDynamics::Model());
246 : 2 : std::string path_to_urdf = ament_index_cpp::get_package_share_directory("rdl_urdfreader") + "/tests/urdf/floating_joint_robot.urdf";
247 : 1 : EXPECT_TRUE(RobotDynamics::Urdf::urdfReadFromFile(path_to_urdf, model));
248 : :
249 : 1 : EXPECT_EQ(model->mJoints[1].mJointType, RobotDynamics::JointTypeTranslationXYZ);
250 : 1 : EXPECT_EQ(model->mJoints[2].mJointType, RobotDynamics::JointTypeEulerXYZ);
251 : :
252 : 2 : std::string contents = getFileContents(path_to_urdf);
253 : 2 : std::vector<unsigned int> q_indices;
254 : 1 : EXPECT_TRUE(RobotDynamics::Urdf::parseJointAndQIndex(contents, q_indices));
255 : :
256 : 1 : EXPECT_EQ(q_indices.size(), (unsigned int)1);
257 : 1 : EXPECT_EQ(q_indices[0], (unsigned int)6);
258 : 1 : }
259 : :
260 : 2 : TEST_F(UrdfReaderTests, testRobotSingleBodyFloatingBase)
261 : : {
262 : 1 : RobotDynamics::ModelPtr model(new RobotDynamics::Model());
263 : 2 : std::string path_to_urdf = ament_index_cpp::get_package_share_directory("rdl_urdfreader") + "/tests/urdf/single_body_floating_base.urdf";
264 : 1 : ASSERT_TRUE(RobotDynamics::Urdf::urdfReadFromFile(path_to_urdf, model));
265 : :
266 : 2 : std::string contents = getFileContents(path_to_urdf);
267 : 2 : std::vector<unsigned int> q_indices;
268 : 1 : EXPECT_TRUE(RobotDynamics::Urdf::parseJointAndQIndex(contents, q_indices));
269 : :
270 : 1 : EXPECT_EQ(q_indices.size(), (unsigned int)0);
271 : 1 : }
272 : :
273 : 2 : TEST_F(UrdfReaderTests, rotated_inertial)
274 : : {
275 : 1 : RobotDynamics::ModelPtr model(new RobotDynamics::Model());
276 : 2 : std::string path_to_urdf = ament_index_cpp::get_package_share_directory("rdl_urdfreader") + "/tests/urdf/rotated_inertial.urdf";
277 : 1 : ASSERT_TRUE(RobotDynamics::Urdf::urdfReadFromFile(path_to_urdf, model));
278 : :
279 : 1 : RobotDynamics::Math::Matrix3d I1 = RobotDynamics::Math::Matrix3dIdentity;
280 : 1 : I1(0, 0) = 0.03;
281 : 1 : I1(1, 1) = 0.03;
282 : 1 : I1(2, 2) = 0.02;
283 : 1 : EXPECT_TRUE(I1.isApprox(model->mBodies[1].mInertia, 1.e-10));
284 : :
285 : 1 : RobotDynamics::Math::Matrix3d I2 = RobotDynamics::Math::Matrix3dIdentity;
286 : 1 : I2(0, 0) = 0.03;
287 : 1 : I2(1, 1) = 0.02;
288 : 1 : I2(2, 2) = 0.03;
289 : 1 : EXPECT_TRUE(I2.isApprox(model->mBodies[2].mInertia, 1.e-10));
290 : :
291 : 1 : RobotDynamics::Math::Matrix3d I3 = RobotDynamics::Math::Matrix3dIdentity;
292 : 1 : I3(0, 0) = 0.02;
293 : 1 : I3(1, 1) = 0.03;
294 : 1 : I3(2, 2) = 0.03;
295 : 1 : EXPECT_TRUE(I3.isApprox(model->mBodies[3].mInertia, 1.e-10));
296 : 1 : }
297 : :
298 : 2 : TEST_F(UrdfReaderTests, rotated_inertial_floating_base)
299 : : {
300 : 1 : RobotDynamics::ModelPtr model(new RobotDynamics::Model());
301 : 2 : std::string path_to_urdf = ament_index_cpp::get_package_share_directory("rdl_urdfreader") + "/tests/urdf/floating_base_rotated_inertial.urdf";
302 : 1 : ASSERT_TRUE(RobotDynamics::Urdf::urdfReadFromFile(path_to_urdf, model));
303 : :
304 : 1 : RobotDynamics::Math::Matrix3d I1 = RobotDynamics::Math::Matrix3dIdentity;
305 : 1 : I1(0, 0) = 0.03;
306 : 1 : I1(1, 1) = 0.03;
307 : 1 : I1(2, 2) = 0.02;
308 : 1 : EXPECT_TRUE(I1.isApprox(model->mBodies[2].mInertia, 1.e-10));
309 : :
310 : 1 : RobotDynamics::Math::Matrix3d I2 = RobotDynamics::Math::Matrix3dIdentity;
311 : 1 : I2(0, 0) = 0.03;
312 : 1 : I2(1, 1) = 0.02;
313 : 1 : I2(2, 2) = 0.03;
314 : 1 : EXPECT_TRUE(I2.isApprox(model->mBodies[3].mInertia, 1.e-10));
315 : :
316 : 1 : RobotDynamics::Math::Matrix3d I3 = RobotDynamics::Math::Matrix3dIdentity;
317 : 1 : I3(0, 0) = 0.02;
318 : 1 : I3(1, 1) = 0.03;
319 : 1 : I3(2, 2) = 0.03;
320 : 1 : EXPECT_TRUE(I3.isApprox(model->mBodies[4].mInertia, 1.e-10));
321 : 1 : }
322 : :
323 : 4 : RobotDynamics::Math::SpatialMatrix reorgAddedInertia(const RobotDynamics::Math::SpatialMatrix& in)
324 : : {
325 : 4 : RobotDynamics::Math::SpatialMatrix out;
326 : 4 : out.block<3, 3>(0, 0) = in.block<3, 3>(3, 3);
327 : 4 : out.block<3, 3>(3, 0) = in.block<3, 3>(0, 3);
328 : 4 : out.block<3, 3>(0, 3) = in.block<3, 3>(3, 0);
329 : 4 : out.block<3, 3>(3, 3) = in.block<3, 3>(0, 0);
330 : 4 : return out;
331 : : }
332 : :
333 : 2 : TEST_F(UrdfReaderTests, parseHydrodynamicsParameters)
334 : : {
335 : 2 : std::string file_path = ament_index_cpp::get_package_share_directory("rdl_urdfreader") + "/tests/urdf/test_robot_arm_hydro.urdf";
336 : 1 : RobotDynamics::Model model;
337 : 1 : std::map<std::string, RobotDynamics::Urdf::hydro::BodyHydroData> data;
338 : 1 : EXPECT_TRUE(RobotDynamics::Urdf::hydro::parseHydrodynamicsParameters(getFileContents(file_path).c_str(), data, false));
339 : :
340 : 1 : EXPECT_FALSE(data["gripper_right_finger_link"].hasHydro);
341 : :
342 : 0 : RobotDynamics::Math::SpatialMatrix I(1., 0., 0., 1.03, 0., 0., 0., 183., 0., 0., 0., 0., 0., 0., 184., 0., 0., 11.133, 0., 0., 0., 2., 0., 0., 0., 2.033, 0., 0., 44.,
343 : 1 : 0., 0., 1.94, 0., 0., 0., 45.311);
344 : :
345 : 2 : EXPECT_TRUE(data["gripper_left_finger_tip_link"].hasHydro);
346 : 2 : EXPECT_EQ(data["gripper_left_finger_tip_link"].volume, 0.003);
347 : 2 : EXPECT_EQ(data["gripper_left_finger_tip_link"].centerOfBuoyancy.x(), 0.01);
348 : 2 : EXPECT_EQ(data["gripper_left_finger_tip_link"].centerOfBuoyancy.y(), 0.21);
349 : 2 : EXPECT_EQ(data["gripper_left_finger_tip_link"].centerOfBuoyancy.z(), -0.03);
350 : 2 : EXPECT_EQ(data["gripper_left_finger_tip_link"].linearDrag[3], 1.);
351 : 2 : EXPECT_EQ(data["gripper_left_finger_tip_link"].linearDrag[4], 2.);
352 : 2 : EXPECT_EQ(data["gripper_left_finger_tip_link"].linearDrag[5], 3.);
353 : 2 : EXPECT_EQ(data["gripper_left_finger_tip_link"].linearDrag[0], 4.);
354 : 2 : EXPECT_EQ(data["gripper_left_finger_tip_link"].linearDrag[1], 5.);
355 : 2 : EXPECT_EQ(data["gripper_left_finger_tip_link"].linearDrag[2], 6.);
356 : 2 : EXPECT_EQ(data["gripper_left_finger_tip_link"].quadraticDrag[3], -1.);
357 : 2 : EXPECT_EQ(data["gripper_left_finger_tip_link"].quadraticDrag[4], -2.);
358 : 2 : EXPECT_EQ(data["gripper_left_finger_tip_link"].quadraticDrag[5], -3.);
359 : 2 : EXPECT_EQ(data["gripper_left_finger_tip_link"].quadraticDrag[0], -4.);
360 : 2 : EXPECT_EQ(data["gripper_left_finger_tip_link"].quadraticDrag[1], -5.);
361 : 2 : EXPECT_EQ(data["gripper_left_finger_tip_link"].quadraticDrag[2], -6.);
362 : 1 : EXPECT_TRUE(reorgAddedInertia(I).isApprox(data["gripper_left_finger_tip_link"].addedMassMatrix, E_MINUS_14));
363 : :
364 : 0 : I = RobotDynamics::Math::SpatialMatrix(1., 0., 0., 0., 0., 0., 0., 183., 0., 0., 0., 0., 0., 0., 184., 0., 0., 0.01, 0., 0., 0., 2., 0., 0., 0., 0., 0., 0., 44., 0.,
365 : 1 : 11., 0., 0., 0., 0., 45.);
366 : 2 : EXPECT_TRUE(data["gripper_right_finger_tip_link"].hasHydro);
367 : 2 : EXPECT_EQ(data["gripper_right_finger_tip_link"].volume, 0.00005);
368 : 2 : EXPECT_EQ(data["gripper_right_finger_tip_link"].centerOfBuoyancy.x(), 0.11);
369 : 2 : EXPECT_EQ(data["gripper_right_finger_tip_link"].centerOfBuoyancy.y(), 1.21);
370 : 2 : EXPECT_EQ(data["gripper_right_finger_tip_link"].centerOfBuoyancy.z(), -0.13);
371 : 2 : EXPECT_EQ(data["gripper_right_finger_tip_link"].linearDrag[3], 10.);
372 : 2 : EXPECT_EQ(data["gripper_right_finger_tip_link"].linearDrag[4], 20.);
373 : 2 : EXPECT_EQ(data["gripper_right_finger_tip_link"].linearDrag[5], 30.);
374 : 2 : EXPECT_EQ(data["gripper_right_finger_tip_link"].linearDrag[0], 40.);
375 : 2 : EXPECT_EQ(data["gripper_right_finger_tip_link"].linearDrag[1], 50.);
376 : 2 : EXPECT_EQ(data["gripper_right_finger_tip_link"].linearDrag[2], 60.);
377 : 2 : EXPECT_EQ(data["gripper_right_finger_tip_link"].quadraticDrag[3], -10.);
378 : 2 : EXPECT_EQ(data["gripper_right_finger_tip_link"].quadraticDrag[4], -20.);
379 : 2 : EXPECT_EQ(data["gripper_right_finger_tip_link"].quadraticDrag[5], -30.);
380 : 2 : EXPECT_EQ(data["gripper_right_finger_tip_link"].quadraticDrag[0], -40.);
381 : 2 : EXPECT_EQ(data["gripper_right_finger_tip_link"].quadraticDrag[1], -50.);
382 : 2 : EXPECT_EQ(data["gripper_right_finger_tip_link"].quadraticDrag[2], -60.);
383 : 1 : EXPECT_TRUE(reorgAddedInertia(I).isApprox(data["gripper_right_finger_tip_link"].addedMassMatrix, E_MINUS_14));
384 : :
385 : 0 : I = RobotDynamics::Math::SpatialMatrix(1., 0., 0., 0., 0., 0., 0., 183., 0., 33.1, 0., 0., 0., 0., 184., 0., 0., 0., 0., 0., 0., 2., 3.11, 0., 0., 0., 0., 0., 44.,
386 : 1 : 0., 0., 0., 0., 0., 0., 45.);
387 : :
388 : 2 : EXPECT_TRUE(data["gripper_right_inner_knuckle_link"].hasHydro);
389 : 2 : EXPECT_EQ(data["gripper_right_inner_knuckle_link"].volume, 0.1);
390 : 2 : EXPECT_EQ(data["gripper_right_inner_knuckle_link"].centerOfBuoyancy.x(), 0.01);
391 : 2 : EXPECT_EQ(data["gripper_right_inner_knuckle_link"].centerOfBuoyancy.y(), 0.21);
392 : 2 : EXPECT_EQ(data["gripper_right_inner_knuckle_link"].centerOfBuoyancy.z(), -0.03);
393 : 2 : EXPECT_EQ(data["gripper_right_inner_knuckle_link"].linearDrag[0], 0.);
394 : 2 : EXPECT_EQ(data["gripper_right_inner_knuckle_link"].linearDrag[1], 0.);
395 : 2 : EXPECT_EQ(data["gripper_right_inner_knuckle_link"].linearDrag[2], 0.);
396 : 2 : EXPECT_EQ(data["gripper_right_inner_knuckle_link"].linearDrag[3], 0.);
397 : 2 : EXPECT_EQ(data["gripper_right_inner_knuckle_link"].linearDrag[4], 0.);
398 : 2 : EXPECT_EQ(data["gripper_right_inner_knuckle_link"].linearDrag[5], 0.);
399 : 2 : EXPECT_EQ(data["gripper_right_inner_knuckle_link"].quadraticDrag[0], 0.);
400 : 2 : EXPECT_EQ(data["gripper_right_inner_knuckle_link"].quadraticDrag[1], 0.);
401 : 2 : EXPECT_EQ(data["gripper_right_inner_knuckle_link"].quadraticDrag[2], 0.);
402 : 2 : EXPECT_EQ(data["gripper_right_inner_knuckle_link"].quadraticDrag[3], 0.);
403 : 2 : EXPECT_EQ(data["gripper_right_inner_knuckle_link"].quadraticDrag[4], 0.);
404 : 2 : EXPECT_EQ(data["gripper_right_inner_knuckle_link"].quadraticDrag[5], 0.);
405 : 1 : EXPECT_TRUE(reorgAddedInertia(I).isApprox(data["gripper_right_inner_knuckle_link"].addedMassMatrix, E_MINUS_14));
406 : :
407 : 1 : I.setZero();
408 : 2 : EXPECT_TRUE(data["gripper_left_finger_link"].hasHydro);
409 : 2 : EXPECT_EQ(data["gripper_left_finger_link"].volume, 0.1);
410 : 2 : EXPECT_EQ(data["gripper_left_finger_link"].centerOfBuoyancy.x(), 0.01);
411 : 2 : EXPECT_EQ(data["gripper_left_finger_link"].centerOfBuoyancy.y(), 0.21);
412 : 2 : EXPECT_EQ(data["gripper_left_finger_link"].centerOfBuoyancy.z(), -0.03);
413 : 2 : EXPECT_EQ(data["gripper_left_finger_link"].linearDrag[0], 0.);
414 : 2 : EXPECT_EQ(data["gripper_left_finger_link"].linearDrag[1], 0.);
415 : 2 : EXPECT_EQ(data["gripper_left_finger_link"].linearDrag[2], 0.);
416 : 2 : EXPECT_EQ(data["gripper_left_finger_link"].linearDrag[3], 0.);
417 : 2 : EXPECT_EQ(data["gripper_left_finger_link"].linearDrag[4], 0.);
418 : 2 : EXPECT_EQ(data["gripper_left_finger_link"].linearDrag[5], 0.);
419 : 2 : EXPECT_EQ(data["gripper_left_finger_link"].quadraticDrag[0], 0.);
420 : 2 : EXPECT_EQ(data["gripper_left_finger_link"].quadraticDrag[1], 0.);
421 : 2 : EXPECT_EQ(data["gripper_left_finger_link"].quadraticDrag[2], 0.);
422 : 2 : EXPECT_EQ(data["gripper_left_finger_link"].quadraticDrag[3], 0.);
423 : 2 : EXPECT_EQ(data["gripper_left_finger_link"].quadraticDrag[4], 0.);
424 : 2 : EXPECT_EQ(data["gripper_left_finger_link"].quadraticDrag[5], 0.);
425 : 1 : EXPECT_TRUE(data["gripper_left_finger_link"].addedMassMatrix.isZero(E_MINUS_14));
426 : :
427 : 0 : I = RobotDynamics::Math::SpatialMatrix(1., 0., 0., 0., 0., 0., 0., 183., 0., 0., 0., 0., 0., 0., 184., 0., 0., 0., 0., 0., 0., 2., 0., 0., 0., 0., 0., 0., 44., 0.,
428 : 1 : 0., 0., 0., 0., 0., 45.);
429 : :
430 : 2 : EXPECT_TRUE(data["gripper_left_knuckle_link"].hasHydro);
431 : 2 : EXPECT_EQ(data["gripper_left_knuckle_link"].volume, 0.);
432 : 2 : EXPECT_EQ(data["gripper_left_knuckle_link"].centerOfBuoyancy.x(), 0.);
433 : 2 : EXPECT_EQ(data["gripper_left_knuckle_link"].centerOfBuoyancy.y(), 0.);
434 : 2 : EXPECT_EQ(data["gripper_left_knuckle_link"].centerOfBuoyancy.z(), 0.);
435 : 2 : EXPECT_EQ(data["gripper_left_knuckle_link"].linearDrag[0], 0.);
436 : 2 : EXPECT_EQ(data["gripper_left_knuckle_link"].linearDrag[1], 0.);
437 : 2 : EXPECT_EQ(data["gripper_left_knuckle_link"].linearDrag[2], 0.);
438 : 2 : EXPECT_EQ(data["gripper_left_knuckle_link"].linearDrag[3], 0.);
439 : 2 : EXPECT_EQ(data["gripper_left_knuckle_link"].linearDrag[4], 0.);
440 : 2 : EXPECT_EQ(data["gripper_left_knuckle_link"].linearDrag[5], 0.);
441 : 2 : EXPECT_EQ(data["gripper_left_knuckle_link"].quadraticDrag[0], 0.);
442 : 2 : EXPECT_EQ(data["gripper_left_knuckle_link"].quadraticDrag[1], 0.);
443 : 2 : EXPECT_EQ(data["gripper_left_knuckle_link"].quadraticDrag[2], 0.);
444 : 2 : EXPECT_EQ(data["gripper_left_knuckle_link"].quadraticDrag[3], 0.);
445 : 2 : EXPECT_EQ(data["gripper_left_knuckle_link"].quadraticDrag[4], 0.);
446 : 2 : EXPECT_EQ(data["gripper_left_knuckle_link"].quadraticDrag[5], 0.);
447 : 1 : EXPECT_TRUE(reorgAddedInertia(I).isApprox(data["gripper_left_knuckle_link"].addedMassMatrix, E_MINUS_14));
448 : 1 : }
449 : :
450 : 2 : TEST_F(UrdfReaderTests, parseHydrodynamicsParametersFailureCases)
451 : : {
452 : 2 : std::string file_path = ament_index_cpp::get_package_share_directory("rdl_urdfreader") + "/tests/urdf/invalid_hydro_added_mass.urdf";
453 : 1 : std::map<std::string, RobotDynamics::Urdf::hydro::BodyHydroData> data;
454 : 1 : EXPECT_FALSE(RobotDynamics::Urdf::hydro::parseHydrodynamicsParameters(getFileContents(file_path).c_str(), data, false));
455 : :
456 : 1 : file_path = ament_index_cpp::get_package_share_directory("rdl_urdfreader") + "/tests/urdf/invalid_drag_block.urdf";
457 : 1 : EXPECT_FALSE(RobotDynamics::Urdf::hydro::parseHydrodynamicsParameters(getFileContents(file_path).c_str(), data, false));
458 : :
459 : 1 : file_path = ament_index_cpp::get_package_share_directory("rdl_urdfreader") + "/tests/urdf/invalid_xml.urdf";
460 : 1 : EXPECT_FALSE(RobotDynamics::Urdf::hydro::parseHydrodynamicsParameters(getFileContents(file_path).c_str(), data, false));
461 : 1 : }
462 : :
463 : 2 : TEST_F(UrdfReaderTests, checkHydroBodiesHaveCorrectHydroInfo)
464 : : {
465 : 2 : std::string file_path = ament_index_cpp::get_package_share_directory("rdl_urdfreader") + "/tests/urdf/test_robot_arm_hydro.urdf";
466 : 1 : RobotDynamics::ModelPtr model(new RobotDynamics::Model());
467 : 1 : EXPECT_TRUE(RobotDynamics::Urdf::urdfReadFromFile(file_path, model));
468 : :
469 : 0 : RobotDynamics::Math::SpatialMatrix I(2., 0., 0., 0., 0., 0., 0., 44., 0., 0., 0., 0., 0., 0., 45., 0., 0., 0., 0., 0., 0., 1., 0., 0., 0., 0., 0., 0., 183.0, 0., 0.,
470 : 1 : 0., 0., 0., 0., 184.);
471 : :
472 : 1 : EXPECT_EQ(model->mBodies[model->GetBodyId("test_robot_shoulder_link")].volume, 0.1);
473 : 1 : EXPECT_EQ(model->mBodies[model->GetBodyId("test_robot_shoulder_link")].mCenterOfBuoyancy.x(), 0.1101);
474 : 1 : EXPECT_EQ(model->mBodies[model->GetBodyId("test_robot_shoulder_link")].mCenterOfBuoyancy.y(), 0.121);
475 : 1 : EXPECT_EQ(model->mBodies[model->GetBodyId("test_robot_shoulder_link")].mCenterOfBuoyancy.z(), -0.013);
476 : :
477 : 1 : EXPECT_TRUE(I.isApprox(model->mBodies[model->GetBodyId("gripper_left_knuckle_link")].addedMassMatrix, E_MINUS_14));
478 : 1 : }
479 : :
480 : : /**
481 : : * The following unit test comes from running a simulation and comparing Adot with a numerically differentiated A matrix. The error
482 : : * between the two was never really greater than about 1.e-4 which seemed sufficient to me so I copied the matrices over here and
483 : : * made this test which until a better test method is found will have to do.
484 : : */
485 : 2 : TEST_F(UrdfReaderTests, centroidalMomentumMatrixDot)
486 : : {
487 : 2 : std::string file_path = ament_index_cpp::get_package_share_directory("rdl_urdfreader") + "/tests/urdf/test_centroidal_momentum_robot.urdf";
488 : 1 : RobotDynamics::ModelPtr model(new RobotDynamics::Model());
489 : 1 : EXPECT_TRUE(RobotDynamics::Urdf::urdfReadFromFile(file_path, model));
490 : 1 : model->gravity = RobotDynamics::Math::MotionVector(0., 0., 0., 0., 0., -9.81);
491 : :
492 : 2 : RobotDynamics::Math::VectorNd q(model->q_size), qdot(model->qdot_size);
493 : :
494 : 1 : q << 2843548.033249, -209909.31618248, -28.737112471808, 0.013866084594564, -0.018922989600583, -0.27902392880053, 46.988201887017, 167.71435783402,
495 : 1 : 0.012894144231981, 0.003644780816261, 0.023437157143762, 0.17174982556876, -272.04399437984, -607.14717812687, 330.80048156122, -356.5405073459, 389.03904152552,
496 : 1 : 0.0014068227420712, -177.3187382107, 0.95999755172569;
497 : 1 : qdot << 0.72356305088634, -0.27922136265996, -0.074898891065314, 0.057378004355399, 0.027501228787648, -0.11882428506134, 1.5988661473426e-05, 8.1292436215699e-06,
498 : 1 : 0.0004935993483221, -1.2482539961087e-05, 4.493479337845e-06, 2.1550289263633e-05, -5.527300395089e-06, 2.0919225428219e-05, 2.0919225428732e-05,
499 : 1 : 1.500606255915e-05, 7.1466266552134e-06, -0.0050623463209877, -1.1308026111878e-06;
500 : 1 : RobotDynamics::updateKinematicsCustom(*model, &q, &qdot, nullptr);
501 : :
502 : 2 : RobotDynamics::Math::MatrixNd adot(6, model->qdot_size), a(6, model->qdot_size), adot2(6, model->qdot_size), a2(6, model->qdot_size);
503 : 1 : RobotDynamics::Math::SpatialVector adotv(81.38892596978, 134.45726399483, 41.428640871662, 75.132338829693, -60.321498882245, 47.201591640815);
504 : 1 : RobotDynamics::Utils::calcCentroidalMomentumMatrix(*model, q, a, false);
505 : 1 : RobotDynamics::Utils::calcCentroidalMomentumMatrixDot(*model, q, qdot, adot, false);
506 : :
507 : 1 : EXPECT_TRUE(adotv.isApprox(adot * qdot, 1.e-7));
508 : :
509 : 1 : RobotDynamics::Utils::calcCentroidalMomentumMatrixAndMatrixDot(*model, q, qdot, a2, adot2, false);
510 : 1 : EXPECT_TRUE(a.isApprox(a2, 1.e-14));
511 : 1 : EXPECT_TRUE(adot.isApprox(adot2, 1.e-14));
512 : :
513 : 1 : q << 2843555.7426166, -209906.42505932, -31.984289527845, -0.019318359864448, 0.084947029667317, 0.38843943516417, 261.52886515801, -370.40405071333, 0.5104959350258,
514 : 1 : 1.1892981440042, -0.74378863055244, 0.88420467075226, 265.39312387749, -419.37422448213, 507.66603672344, -672.60712542416, 608.34632442873, 7.2151029691512e-06,
515 : 1 : 241.8163217398, 0.91734704901263;
516 : 1 : qdot << 0.90780498518421, 0.37795991108503, -0.12286976779791, 0.016129512038977, 0.011259829695686, 0.10567671184028, 1.7140545707062e-05, 1.0042944296275e-05,
517 : 1 : 0.06211524064157, 0.12779617820982, -0.088022453340217, 0.096940404958886, -4.9915274408924e-06, 2.3314482437527e-05, 2.3314482437333e-05, 1.5741349596001e-05,
518 : 1 : 8.6437310570578e-06, -0.0021085140258945, -1.6102261528733e-06;
519 : 1 : RobotDynamics::updateKinematicsCustom(*model, &q, &qdot, nullptr);
520 : :
521 : 1 : adotv << 29.80644161631, -26.852723901131, 4.1278484858571, 47.672589300394, 48.762586239518, -22.769718014228;
522 : 1 : RobotDynamics::Utils::calcCentroidalMomentumMatrix(*model, q, a, false);
523 : 1 : RobotDynamics::Utils::calcCentroidalMomentumMatrixDot(*model, q, qdot, adot, false);
524 : :
525 : 1 : EXPECT_TRUE(adotv.isApprox(adot * qdot, 1.e-7));
526 : 1 : RobotDynamics::Utils::calcCentroidalMomentumMatrixAndMatrixDot(*model, q, qdot, a2, adot2, false);
527 : 1 : EXPECT_TRUE(a.isApprox(a2, 1.e-14));
528 : 1 : EXPECT_TRUE(adot.isApprox(adot2, 1.e-14));
529 : 1 : }
530 : :
531 : 2 : TEST_F(UrdfReaderTests, eulerzyx_coordinates_for_floating_base)
532 : : {
533 : 2 : std::string file_path = ament_index_cpp::get_package_share_directory("rdl_urdfreader") + "/tests/urdf/test_centroidal_momentum_robot.urdf";
534 : 2 : RobotDynamics::ModelPtr euler_model(new RobotDynamics::Model()), fb_model(new RobotDynamics::Model());
535 : 1 : EXPECT_TRUE(RobotDynamics::Urdf::urdfReadFromFile(file_path, euler_model, RobotDynamics::JointTypeEulerZYX));
536 : 1 : EXPECT_TRUE(RobotDynamics::Urdf::urdfReadFromFile(file_path, fb_model));
537 : :
538 : 2 : RobotDynamics::Math::VectorNd q_eul(euler_model->q_size), qdot_eul(euler_model->qdot_size);
539 : 2 : RobotDynamics::Math::VectorNd q_fb(fb_model->q_size), qdot_fb(fb_model->qdot_size);
540 : :
541 : : // randomize the fb state
542 : 20 : for (int i = 0; i < q_eul.size(); ++i)
543 : : {
544 : 19 : q_eul[i] = M_PI * (static_cast<double>(rand()) / static_cast<double>(RAND_MAX)) - M_PI_2;
545 : 19 : qdot_eul[i] = M_PI * (static_cast<double>(rand()) / static_cast<double>(RAND_MAX)) - M_PI_2;
546 : 19 : q_fb[i] = q_eul[i];
547 : 19 : qdot_fb[i] = qdot_eul[i];
548 : : }
549 : :
550 : 1 : unsigned int euler_base_link_id = euler_model->GetBodyId("base_link");
551 : 1 : unsigned int fb_base_link_id = fb_model->GetBodyId("base_link");
552 : :
553 : : // now fix the base link orientation bits since they have different orientation coordinates for the base link
554 : 1 : RobotDynamics::Math::Quaternion o(RobotDynamics::Math::XeulerZYX(q_eul[3], q_eul[4], q_eul[5]).E);
555 : 1 : fb_model->SetQuaternion(fb_base_link_id, o, q_fb);
556 : :
557 : 1 : RobotDynamics::Math::Vector3d w = RobotDynamics::Math::angular_velocity_from_zyx_angle_rates(RobotDynamics::Math::Vector3d(q_eul[3], q_eul[4], q_eul[5]),
558 : 2 : RobotDynamics::Math::Vector3d(qdot_eul[3], qdot_eul[4], qdot_eul[5]));
559 : :
560 : 1 : qdot_fb[3] = w[0];
561 : 1 : qdot_fb[4] = w[1];
562 : 1 : qdot_fb[5] = w[2];
563 : : // now update both kinematics and check that they are the same
564 : 1 : RobotDynamics::updateKinematicsCustom(*euler_model, &q_eul, &qdot_eul);
565 : 1 : RobotDynamics::updateKinematicsCustom(*fb_model, &q_fb, &qdot_fb);
566 : :
567 : 17 : for (size_t i = 0; i < fb_model->bodyFrames.size(); ++i)
568 : : {
569 : 16 : EXPECT_TRUE(fb_model->bodyFrames[i]->getTransformToRoot().toMatrix().isApprox(euler_model->bodyFrames[i]->getTransformToRoot().toMatrix(), 1.e-14));
570 : : }
571 : :
572 : 17 : for (size_t i = 0; i < fb_model->bodyCenteredFrames.size(); ++i)
573 : : {
574 : 16 : EXPECT_TRUE(
575 : 16 : fb_model->bodyCenteredFrames[i]->getTransformToRoot().toMatrix().isApprox(euler_model->bodyCenteredFrames[i]->getTransformToRoot().toMatrix(), 1.e-14));
576 : : }
577 : 1 : EXPECT_TRUE(fb_model->v[fb_base_link_id].isApprox(euler_model->v[euler_base_link_id], 1.e-14));
578 : 1 : }
579 : :
580 : 2 : TEST_F(UrdfReaderTests, eulerxyz_coordinates_for_floating_base)
581 : : {
582 : 2 : std::string file_path = ament_index_cpp::get_package_share_directory("rdl_urdfreader") + "/tests/urdf/test_centroidal_momentum_robot.urdf";
583 : 2 : RobotDynamics::ModelPtr euler_model(new RobotDynamics::Model()), fb_model(new RobotDynamics::Model());
584 : 1 : EXPECT_TRUE(RobotDynamics::Urdf::urdfReadFromFile(file_path, euler_model, RobotDynamics::JointTypeEulerXYZ));
585 : 1 : EXPECT_TRUE(RobotDynamics::Urdf::urdfReadFromFile(file_path, fb_model));
586 : :
587 : 2 : RobotDynamics::Math::VectorNd q_eul(euler_model->q_size), qdot_eul(euler_model->qdot_size);
588 : 2 : RobotDynamics::Math::VectorNd q_fb(fb_model->q_size), qdot_fb(fb_model->qdot_size);
589 : :
590 : : // randomize the fb state
591 : 20 : for (int i = 0; i < q_eul.size(); ++i)
592 : : {
593 : 19 : q_eul[i] = M_PI * (static_cast<double>(rand()) / static_cast<double>(RAND_MAX)) - M_PI_2;
594 : 19 : qdot_eul[i] = M_PI * (static_cast<double>(rand()) / static_cast<double>(RAND_MAX)) - M_PI_2;
595 : 19 : q_fb[i] = q_eul[i];
596 : 19 : qdot_fb[i] = qdot_eul[i];
597 : : }
598 : :
599 : 1 : unsigned int euler_base_link_id = euler_model->GetBodyId("base_link");
600 : 1 : unsigned int fb_base_link_id = fb_model->GetBodyId("base_link");
601 : :
602 : : // now fix the base link orientation bits since they have different orientation coordinates for the base link
603 : 1 : RobotDynamics::Math::Quaternion o(RobotDynamics::Math::XeulerXYZ(q_eul[3], q_eul[4], q_eul[5]).E);
604 : 1 : fb_model->SetQuaternion(fb_base_link_id, o, q_fb);
605 : :
606 : 1 : RobotDynamics::Math::Vector3d w = RobotDynamics::Math::angular_velocity_from_xyz_angle_rates(RobotDynamics::Math::Vector3d(q_eul[3], q_eul[4], q_eul[5]),
607 : 2 : RobotDynamics::Math::Vector3d(qdot_eul[3], qdot_eul[4], qdot_eul[5]));
608 : :
609 : 1 : qdot_fb[3] = w[0];
610 : 1 : qdot_fb[4] = w[1];
611 : 1 : qdot_fb[5] = w[2];
612 : : // now update both kinematics and check that they are the same
613 : 1 : RobotDynamics::updateKinematicsCustom(*euler_model, &q_eul, &qdot_eul);
614 : 1 : RobotDynamics::updateKinematicsCustom(*fb_model, &q_fb, &qdot_fb);
615 : :
616 : 17 : for (size_t i = 0; i < fb_model->bodyFrames.size(); ++i)
617 : : {
618 : 16 : EXPECT_TRUE(fb_model->bodyFrames[i]->getTransformToRoot().toMatrix().isApprox(euler_model->bodyFrames[i]->getTransformToRoot().toMatrix(), 1.e-14));
619 : : }
620 : :
621 : 17 : for (size_t i = 0; i < fb_model->bodyCenteredFrames.size(); ++i)
622 : : {
623 : 16 : EXPECT_TRUE(
624 : 16 : fb_model->bodyCenteredFrames[i]->getTransformToRoot().toMatrix().isApprox(euler_model->bodyCenteredFrames[i]->getTransformToRoot().toMatrix(), 1.e-14));
625 : : }
626 : 1 : EXPECT_TRUE(fb_model->v[fb_base_link_id].isApprox(euler_model->v[euler_base_link_id], 1.e-14));
627 : 1 : }
628 : :
629 : 2 : TEST_F(UrdfReaderTests, load_ros_params_from_file)
630 : : {
631 : 2 : std::string filepath = ament_index_cpp::get_package_share_directory("rdl_urdfreader") + "/tests/config/load_ros_params_from_file.yaml";
632 : 1 : rclcpp::NodeOptions options;
633 : 4 : options.arguments({ "--ros-args", "--params-file", filepath.c_str() });
634 : :
635 : 1 : rclcpp::Node::SharedPtr n = rclcpp::Node::make_shared("load_ros_params_from_file", options);
636 : 1 : RobotDynamics::ModelPtr model(new RobotDynamics::Model(n));
637 : 2 : std::string path_to_urdf = ament_index_cpp::get_package_share_directory("rdl_urdfreader") + "/tests/urdf/floating_base_robot.urdf";
638 : 1 : EXPECT_TRUE(RobotDynamics::Urdf::urdfReadFromFile(path_to_urdf, model));
639 : 1 : model->setupRosParameters();
640 : :
641 : 1 : EXPECT_NEAR(model->mass, 17.778, 1.e-14);
642 : 1 : EXPECT_NEAR(model->volume, 1.1, 1.e-14);
643 : 1 : }
644 : :
645 : 1 : int main(int argc, char** argv)
646 : : {
647 : 1 : ::testing::InitGoogleTest(&argc, argv);
648 : 1 : ::testing::FLAGS_gtest_death_test_style = "threadsafe";
649 : 1 : rclcpp::init(argc, argv);
650 : 1 : return RUN_ALL_TESTS();
651 : : }
|
