@@ -24,19 +24,6 @@ bool quat_are_near(urdf::Rotation left, urdf::Rotation right)
2424 std::abs (l[3 ] + r[3 ]) < epsilon);
2525}
2626
27- std::ostream &operator <<(std::ostream &os, const urdf::Rotation& rot)
28- {
29- double roll, pitch, yaw;
30- double x, y, z, w;
31- rot.getRPY (roll, pitch, yaw);
32- rot.getQuaternion (x, y, z, w);
33- os << std::setprecision (9 )
34- << " x: " << x << " y: " << y << " z: " << z << " w: " << w
35- << " roll: " << roll << " pitch: " << pitch << " yaw: " << yaw;
36- return os;
37- }
38-
39-
4027void check_get_set_rpy_is_idempotent (double x, double y, double z, double w)
4128{
4229 urdf::Rotation rot0;
@@ -45,12 +32,6 @@ void check_get_set_rpy_is_idempotent(double x, double y, double z, double w)
4532 rot0.getRPY (roll, pitch, yaw);
4633 urdf::Rotation rot1;
4734 rot1.setFromRPY (roll, pitch, yaw);
48- if (true ) {
49- std::cout << " \n "
50- << " before " << rot0 << " \n "
51- << " after " << rot1 << " \n "
52- << " ok " << quat_are_near (rot0, rot1) << " \n " ;
53- }
5435 EXPECT_TRUE (quat_are_near (rot0, rot1));
5536}
5637
@@ -63,12 +44,6 @@ void check_get_set_rpy_is_idempotent_from_rpy(double r, double p, double y)
6344 urdf::Rotation rot1;
6445 rot1.setFromRPY (roll, pitch, yaw);
6546 bool ok = quat_are_near (rot0, rot1);
66- if (!ok) {
67- std::cout << " initial rpy: " << r << " " << p << " " << y << " \n "
68- << " before " << rot0 << " \n "
69- << " after " << rot1 << " \n "
70- << " ok " << ok << " \n " ;
71- }
7247 EXPECT_TRUE (ok);
7348}
7449
0 commit comments