@@ -27,19 +27,6 @@ bool quat_are_near(urdf::Rotation left, urdf::Rotation right)
2727 std::abs (l[3 ] + r[3 ]) < epsilon);
2828}
2929
30- std::ostream &operator <<(std::ostream &os, const urdf::Rotation& rot)
31- {
32- double roll, pitch, yaw;
33- double x, y, z, w;
34- rot.getRPY (roll, pitch, yaw);
35- rot.getQuaternion (x, y, z, w);
36- os << std::setprecision (9 )
37- << " x: " << x << " y: " << y << " z: " << z << " w: " << w
38- << " roll: " << roll << " pitch: " << pitch << " yaw: " << yaw;
39- return os;
40- }
41-
42-
4330void check_get_set_rpy_is_idempotent (double x, double y, double z, double w)
4431{
4532 urdf::Rotation rot0;
@@ -48,12 +35,6 @@ void check_get_set_rpy_is_idempotent(double x, double y, double z, double w)
4835 rot0.getRPY (roll, pitch, yaw);
4936 urdf::Rotation rot1;
5037 rot1.setFromRPY (roll, pitch, yaw);
51- if (true ) {
52- std::cout << " \n "
53- << " before " << rot0 << " \n "
54- << " after " << rot1 << " \n "
55- << " ok " << quat_are_near (rot0, rot1) << " \n " ;
56- }
5738 EXPECT_TRUE (quat_are_near (rot0, rot1));
5839}
5940
@@ -66,12 +47,6 @@ void check_get_set_rpy_is_idempotent_from_rpy(double r, double p, double y)
6647 urdf::Rotation rot1;
6748 rot1.setFromRPY (roll, pitch, yaw);
6849 bool ok = quat_are_near (rot0, rot1);
69- if (!ok) {
70- std::cout << " initial rpy: " << r << " " << p << " " << y << " \n "
71- << " before " << rot0 << " \n "
72- << " after " << rot1 << " \n "
73- << " ok " << ok << " \n " ;
74- }
7550 EXPECT_TRUE (ok);
7651}
7752
0 commit comments