diff --git a/dm_control/utils/transformations.py b/dm_control/utils/transformations.py index 74289c35..b59d193c 100644 --- a/dm_control/utils/transformations.py +++ b/dm_control/utils/transformations.py @@ -639,8 +639,10 @@ def axisangle_to_quat(axisangle, tol=0.0): """ axisangle = np.asarray(axisangle) angle = np.linalg.norm(axisangle, axis=-1, keepdims=True) - axis = np.where(angle <= tol, [1.0, 0.0, 0.0], axisangle / angle) - angle = np.where(angle <= tol, [0.0], angle) + is_identity = angle <= tol + safe_angle = np.where(is_identity, 1.0, angle) + axis = np.where(is_identity, [1.0, 0.0, 0.0], axisangle / safe_angle) + angle = np.where(is_identity, [0.0], angle) sine, cosine = np.sin(angle / 2), np.cos(angle / 2) return np.concatenate([cosine, axis * sine], axis=-1) diff --git a/dm_control/utils/transformations_test.py b/dm_control/utils/transformations_test.py index b11a5694..35969773 100644 --- a/dm_control/utils/transformations_test.py +++ b/dm_control/utils/transformations_test.py @@ -268,7 +268,8 @@ def test_axisangle_to_quat(self): def test_axisangle_to_quat_zero(self): axisangle = np.array([0, 0, 0]) - quat = transformations.axisangle_to_quat(axisangle) + with np.errstate(invalid='raise', divide='raise'): + quat = transformations.axisangle_to_quat(axisangle) np.testing.assert_allclose(quat, [1, 0, 0, 0]) def test_axisangle_to_quat_zero_tol(self): @@ -277,12 +278,14 @@ def test_axisangle_to_quat_zero_tol(self): np.testing.assert_allclose(quat, [1, 0, 0, 0]) def test_axisangle_to_quat_batched(self): - axisangle = np.stack([np.array([0.1, 0.2, 0.3]), np.array([0.4, 0.5, 0.6])]) - quat = transformations.axisangle_to_quat(axisangle) + axisangle = np.array([[0.1, 0.2, 0.3], [0, 0, 0], [0.4, 0.5, 0.6]]) + with np.errstate(invalid='raise', divide='raise'): + quat = transformations.axisangle_to_quat(axisangle) np.testing.assert_allclose( quat, [ [0.982551, 0.0497088, 0.0994177, 0.1491265], + [1, 0, 0, 0], [0.9052841, 0.1936448, 0.242056, 0.2904672], ], atol=1e-6,