diff --git a/dm_control/utils/transformations.py b/dm_control/utils/transformations.py index 74289c35..15f17964 100644 --- a/dm_control/utils/transformations.py +++ b/dm_control/utils/transformations.py @@ -116,7 +116,7 @@ def _rmat_to_euler_zyx(rmat): """Converts a 3x3 rotation matrix to ZYX euler angles.""" if rmat[2, 0] > _POLE_LIMIT: logging.warning('Angle at North Pole') - x = np.arctan2(rmat[0, 1], rmat[0, 2]) + x = np.arctan2(-rmat[0, 1], -rmat[0, 2]) y = -np.pi/2 z = 0.0 return np.array([z, y, x]) @@ -140,7 +140,7 @@ def _rmat_to_euler_xzy(rmat): """Converts a 3x3 rotation matrix to XZY euler angles.""" if rmat[0, 1] > _POLE_LIMIT: logging.warning('Angle at North Pole') - y = np.arctan2(rmat[1, 2], rmat[1, 0]) + y = np.arctan2(-rmat[1, 2], -rmat[1, 0]) z = -np.pi/2 x = 0.0 return np.array([x, z, y]) @@ -164,7 +164,7 @@ def _rmat_to_euler_yzx(rmat): """Converts a 3x3 rotation matrix to YZX euler angles.""" if rmat[1, 0] > _POLE_LIMIT: logging.warning('Angle at North Pole') - x = -np.arctan2(rmat[0, 2], rmat[0, 1]) + x = np.arctan2(rmat[0, 2], -rmat[0, 1]) z = np.pi/2 y = 0.0 return np.array([y, z, x]) diff --git a/dm_control/utils/transformations_test.py b/dm_control/utils/transformations_test.py index b11a5694..4c145267 100644 --- a/dm_control/utils/transformations_test.py +++ b/dm_control/utils/transformations_test.py @@ -109,6 +109,16 @@ def test_euler_to_rmat_special(self, angles): euler_angles = transformations.rmat_to_euler(r, ordering) np.testing.assert_allclose(euler_angles, [r1, r2, r3]) + @parameterized.parameters('XYZ', 'ZYX', 'XZY', 'YZX', 'ZXY', 'YXZ') + def test_rmat_to_euler_tait_bryan_singularities(self, ordering): + middle_angles = (-np.pi/2, np.pi/2) + for angles in itertools.product((-.3, .3), middle_angles, (-.7, .7)): + rmat = transformations.euler_to_rmat(angles, ordering) + euler_angles = transformations.rmat_to_euler(rmat, ordering) + # Euler angles are not unique at a singularity, but the rotation is. + reconstructed = transformations.euler_to_rmat(euler_angles, ordering) + np.testing.assert_allclose(reconstructed, rmat, atol=1e-10) + def test_quat_mul_vs_mat_mul_random(self): for _ in range(_NUM_RANDOM_SAMPLES): quat1 = self._random_quaternion()