From 9982d19f999a0bc8e693af2d43cc95c340b5e8d2 Mon Sep 17 00:00:00 2001 From: Sanjeev Kumar Date: Sun, 8 Dec 2024 23:20:46 +0100 Subject: [PATCH] Add more unit tests to verify functionality for angle units as radians or degrees in class --- pyrobosim/test/utils/test_pose_utils.py | 315 +++++++++++++++++++++--- 1 file changed, 285 insertions(+), 30 deletions(-) diff --git a/pyrobosim/test/utils/test_pose_utils.py b/pyrobosim/test/utils/test_pose_utils.py index 087205a8..a54cea4d 100644 --- a/pyrobosim/test/utils/test_pose_utils.py +++ b/pyrobosim/test/utils/test_pose_utils.py @@ -28,6 +28,7 @@ def test_pose_default_args(): assert pose.z == pytest.approx(0.0) assert pose.eul == pytest.approx([0.0, 0.0, 0.0]) assert pose.q == pytest.approx([1.0, 0.0, 0.0, 0.0]) + assert pose.angle_units == "radians" def test_pose_from_position(): @@ -40,13 +41,20 @@ def test_pose_from_position(): assert pose.q == pytest.approx([1.0, 0.0, 0.0, 0.0]) -def test_pose_from_euler_angles(): - """Test creating a pose using Euler angles.""" - pose = Pose(roll=np.pi / 2, pitch=0.0, yaw=-np.pi / 2) +@pytest.mark.parametrize( + "angle_units, roll, pitch, yaw, expected_euler", + [ + ("radians", np.pi / 2, 0.0, -np.pi / 2, [np.pi / 2, 0.0, -np.pi / 2]), + ("degrees", 90, 0, -90, [np.pi / 2, 0.0, -np.pi / 2]), + ], +) +def test_pose_from_euler_angles(angle_units, roll, pitch, yaw, expected_euler): + """Test creating a pose using Euler angles specified in different units.""" + pose = Pose(roll=roll, pitch=pitch, yaw=yaw, angle_units=angle_units) assert pose.x == pytest.approx(0.0) assert pose.y == pytest.approx(0.0) assert pose.z == pytest.approx(0.0) - assert pose.eul == pytest.approx([np.pi / 2, 0.0, -np.pi / 2]) + assert pose.eul == pytest.approx(expected_euler) assert pose.q == pytest.approx([0.5, 0.5, 0.5, -0.5]) @@ -73,21 +81,41 @@ def test_pose_from_lists(): assert pose.y == pytest.approx(2.0) assert pose.z == pytest.approx(3.0) - # 4-element lists should be [x, y, z, yaw] - pose = Pose.from_list([1.0, 2.0, 3.0, np.pi / 2]) + # 4-element lists should be [x, y, z, yaw], angle_units in radians + pose = Pose.from_list([1.0, 2.0, 3.0, np.pi / 2], "radians") + assert pose.x == pytest.approx(1.0) + assert pose.y == pytest.approx(2.0) + assert pose.z == pytest.approx(3.0) + assert pose.eul == pytest.approx([0.0, 0.0, np.pi / 2]) + assert pose.q == pytest.approx([np.sqrt(2) / 2, 0.0, 0.0, np.sqrt(2) / 2]) + assert pose.angle_units == "radians" + + # 4-element lists should be [x, y, z, yaw], angle_units in degrees + pose = Pose.from_list([1.0, 2.0, 3.0, 90], "degrees") assert pose.x == pytest.approx(1.0) assert pose.y == pytest.approx(2.0) assert pose.z == pytest.approx(3.0) assert pose.eul == pytest.approx([0.0, 0.0, np.pi / 2]) assert pose.q == pytest.approx([np.sqrt(2) / 2, 0.0, 0.0, np.sqrt(2) / 2]) + assert pose.angle_units == "degrees" + + # 6-element lists should be [x, y, z, roll, pitch, yaw], angle_units in radians + pose = Pose.from_list([1.0, 2.0, 3.0, np.pi / 2, 0.0, -np.pi / 2], "radians") + assert pose.x == pytest.approx(1.0) + assert pose.y == pytest.approx(2.0) + assert pose.z == pytest.approx(3.0) + assert pose.eul == pytest.approx([np.pi / 2, 0.0, -np.pi / 2]) + assert pose.q == pytest.approx([0.5, 0.5, 0.5, -0.5]) + assert pose.angle_units == "radians" - # 6-element lists should be [x, y, z, roll, pitch, yaw] - pose = Pose.from_list([1.0, 2.0, 3.0, np.pi / 2, 0.0, -np.pi / 2]) + # 6-element lists should be [x, y, z, roll, pitch, yaw], angle_units in degrees + pose = Pose.from_list([1.0, 2.0, 3.0, 90, 0.0, -90], "degrees") assert pose.x == pytest.approx(1.0) assert pose.y == pytest.approx(2.0) assert pose.z == pytest.approx(3.0) assert pose.eul == pytest.approx([np.pi / 2, 0.0, -np.pi / 2]) assert pose.q == pytest.approx([0.5, 0.5, 0.5, -0.5]) + assert pose.angle_units == "degrees" # 7-element lists should be [x, y, z, qw, qx, qy, qz] pose = Pose.from_list([1.0, 2.0, 3.0, 0.5, 0.5, 0.5, -0.5]) @@ -102,6 +130,13 @@ def test_pose_from_lists(): Pose.from_list([1.0, 2.0, 3.0, 4.0, 5.0]) assert exc_info.value.args[0] == "List must contain 2, 3, 4, 6, or 7 elements." + # Invalid angle_units raise an exception + with pytest.raises(ValueError) as exc_info: + Pose(angle_units="notavalidangle") + assert ( + exc_info.value.args[0] == "Angles should be either in 'radians' or 'degrees'." + ) + def test_pose_from_transform(): """Test creating a pose using a transform.""" @@ -122,15 +157,16 @@ def test_pose_from_transform(): assert pose.q == pytest.approx([0.5, 0.5, 0.5, -0.5]) -def test_pose_to_from_dict(): +def test_pose_to_from_dict_angle_in_radians(): """Test creating poses using a dictionary and saving them back out.""" pose_dict = {} - pose = Pose.from_dict(pose_dict) + pose = Pose.from_dict(pose_dict, "radians") assert pose.x == pytest.approx(0.0) assert pose.y == pytest.approx(0.0) assert pose.z == pytest.approx(0.0) assert pose.eul == pytest.approx([0.0, 0.0, 0.0]) assert pose.q == pytest.approx([1.0, 0.0, 0.0, 0.0]) + assert pose.angle_units == "radians" pose_dict = { "position": {"x": 1.0, "y": 2.0, "z": 3.0}, @@ -140,11 +176,12 @@ def test_pose_to_from_dict(): "roll": -np.pi / 2.0, }, } - pose = Pose.from_dict(pose_dict) + pose = Pose.from_dict(pose_dict, angle_units="radians") assert pose.x == pytest.approx(1.0) assert pose.y == pytest.approx(2.0) assert pose.z == pytest.approx(3.0) assert pose.eul == pytest.approx([-np.pi / 2.0, np.pi / 4.0, np.pi / 2.0]) + assert pose.angle_units == "radians" pose_dict = { "position": {"x": 1.0, "y": 2.0, "z": 3.0}, @@ -155,12 +192,77 @@ def test_pose_to_from_dict(): }, "rotation_quat": {"w": 0.707107, "x": -0.707107, "y": 0.0, "z": 0.0}, } - pose = Pose.from_dict(pose_dict) + pose = Pose.from_dict(pose_dict, angle_units="radians") + assert pose.x == pytest.approx(1.0) + assert pose.y == pytest.approx(2.0) + assert pose.z == pytest.approx(3.0) + assert pose.eul == pytest.approx([-np.pi / 2.0, 0.0, 0.0]) + assert pose.q == pytest.approx([0.707107, -0.707107, 0.0, 0.0]) + assert pose.angle_units == "radians" + + out_dict = pose.to_dict() + assert "position" in out_dict + pos = out_dict["position"] + assert "x" in pos + assert pos["x"] == pytest.approx(1.0) + assert "y" in pos + assert pos["y"] == pytest.approx(2.0) + assert "z" in pos + assert pos["z"] == pytest.approx(3.0) + assert "rotation_quat" in out_dict + quat = out_dict["rotation_quat"] + assert "w" in quat + assert quat["w"] == pytest.approx(0.707107) + assert "x" in quat + assert quat["x"] == pytest.approx(-0.707107) + assert "y" in quat + assert quat["y"] == pytest.approx(0.0) + assert "z" in quat + assert quat["z"] == pytest.approx(0.0) + + +def test_pose_to_from_dict_angle_in_degrees(): + """Test creating poses using a dictionary and saving them back out.""" + pose_dict = {} + pose = Pose.from_dict(pose_dict, angle_units="degrees") + assert pose.x == pytest.approx(0.0) + assert pose.y == pytest.approx(0.0) + assert pose.z == pytest.approx(0.0) + assert pose.eul == pytest.approx([0.0, 0.0, 0.0]) + assert pose.q == pytest.approx([1.0, 0.0, 0.0, 0.0]) + assert pose.angle_units == "degrees" + + pose_dict = { + "position": {"x": 1.0, "y": 2.0, "z": 3.0}, + "rotation_eul": { + "yaw": 90, + "pitch": 45, + "roll": -90, + }, + } + pose = Pose.from_dict(pose_dict, angle_units="degrees") + assert pose.x == pytest.approx(1.0) + assert pose.y == pytest.approx(2.0) + assert pose.z == pytest.approx(3.0) + assert pose.eul == pytest.approx([-np.pi / 2.0, np.pi / 4.0, np.pi / 2.0]) + assert pose.angle_units == "degrees" + + pose_dict = { + "position": {"x": 1.0, "y": 2.0, "z": 3.0}, + "rotation_eul": { + "yaw": 90, + "pitch": 45, + "roll": -90, + }, + "rotation_quat": {"w": 0.707107, "x": -0.707107, "y": 0.0, "z": 0.0}, + } + pose = Pose.from_dict(pose_dict, angle_units="degrees") assert pose.x == pytest.approx(1.0) assert pose.y == pytest.approx(2.0) assert pose.z == pytest.approx(3.0) assert pose.eul == pytest.approx([-np.pi / 2.0, 0.0, 0.0]) assert pose.q == pytest.approx([0.707107, -0.707107, 0.0, 0.0]) + assert pose.angle_units == "degrees" out_dict = pose.to_dict() assert "position" in out_dict @@ -183,15 +285,46 @@ def test_pose_to_from_dict(): assert quat["z"] == pytest.approx(0.0) -def test_construct_pose(): +def test_construct_pose_angle_in_radians(): """Test pose construct function that accepts various types.""" - pose_from_list = Pose.construct([1.0, 2.0, 3.0, np.pi / 2.0]) + pose_from_list = Pose.construct([1.0, 2.0, 3.0, np.pi / 2.0], "radians") pose_from_dict = Pose.construct( { "position": {"x": 1.0, "y": 2.0, "z": 3.0}, "rotation_eul": {"yaw": np.pi / 2.0}, - } + }, + "radians", + ) + + tform = np.array( + [ + [0.0, -1.0, 0.0, 1.0], + [1.0, 0.0, 0.0, 2.0], + [0.0, 0.0, 1.0, 3.0], + [0.0, 0.0, 0.0, 1.0], + ] + ) + pose_from_tform = Pose.construct(tform) + + assert pose_from_list.is_approx(pose_from_dict) + assert pose_from_dict.is_approx(pose_from_tform) + + with pytest.raises(ValueError) as exc_info: + Pose.construct(42) + assert exc_info.value.args[0] == "Cannot construct pose from object of type int." + + +def test_construct_pose_angle_in_degrees(): + """Test pose construct function that accepts various types.""" + pose_from_list = Pose.construct([1.0, 2.0, 3.0, 90], "degrees") + + pose_from_dict = Pose.construct( + { + "position": {"x": 1.0, "y": 2.0, "z": 3.0}, + "rotation_eul": {"yaw": 90}, + }, + "degrees", ) tform = np.array( @@ -239,18 +372,57 @@ def test_get_angular_distance(): assert pose1.get_angular_distance(pose2) == pytest.approx(-3 * np.pi / 4) -def test_get_matrices(): - """Tests getting matrices from a pose.""" - pose = Pose(x=1.0, y=2.0, z=3.0, roll=np.pi / 2, pitch=0.0, yaw=-np.pi / 2) - - expected_tform = np.array( - [ - [0.0, 1.0, 0.0, 1.0], - [0.0, 0.0, -1.0, 2.0], - [-1.0, 0.0, 0.0, 3.0], - [0.0, 0.0, 0.0, 1.0], - ] +@pytest.mark.parametrize( + "x, y, z, roll, pitch, yaw, angle_units, expected_tform", + [ + ( + 1.0, + 2.0, + 3.0, + np.pi / 2, + 0.0, + -np.pi / 2, + "radians", + np.array( + [ + [0.0, 1.0, 0.0, 1.0], + [0.0, 0.0, -1.0, 2.0], + [-1.0, 0.0, 0.0, 3.0], + [0.0, 0.0, 0.0, 1.0], + ] + ), + ), + ( + 1.0, + 2.0, + 3.0, + 90, + 0.0, + -90, + "degrees", + np.array( + [ + [0.0, 1.0, 0.0, 1.0], + [0.0, 0.0, -1.0, 2.0], + [-1.0, 0.0, 0.0, 3.0], + [0.0, 0.0, 0.0, 1.0], + ] + ), + ), + ], +) +def test_get_matrices(x, y, z, roll, pitch, yaw, angle_units, expected_tform): + """Test getting matrices from a pose with different angle units""" + pose = Pose( + x=x, + y=y, + z=z, + roll=roll, + pitch=pitch, + yaw=yaw, + angle_units=angle_units, ) + expected_translation_matrix = np.eye(4) expected_translation_matrix[:3, 3] = expected_tform[:3, 3] @@ -259,16 +431,99 @@ def test_get_matrices(): assert pose.get_translation_matrix() == pytest.approx(expected_translation_matrix) -def test_is_approx(): +@pytest.mark.parametrize( + "pose1, pose2, rel_tol, abs_tol, expected_result", + [ + ( + Pose( + x=1.0, + y=2.0, + z=3.0, + roll=np.pi / 2, + pitch=0.0, + yaw=-np.pi / 2, + angle_units="radians", + ), + Pose( + x=1.0 + 1e-4, + y=2.0, + z=3.0, + roll=np.pi / 2, + pitch=1e-4, + yaw=-np.pi / 2, + angle_units="radians", + ), + 1e-3, + 1e-3, + True, + ), + ( + Pose( + x=1.0, + y=2.0, + z=3.0, + roll=np.pi / 2, + pitch=0.0, + yaw=-np.pi / 2, + angle_units="radians", + ), + Pose( + x=1.0 + 1e-4, + y=2.0, + z=3.0, + roll=np.pi / 2, + pitch=1e-4, + yaw=-np.pi / 2, + angle_units="radians", + ), + 1e-5, + 1e-5, + False, + ), + ( + Pose( + x=1.0, y=2.0, z=3.0, roll=90, pitch=0.0, yaw=-90, angle_units="degrees" + ), + Pose( + x=1.0 + 1e-4, + y=2.0, + z=3.0, + roll=90, + pitch=1e-4, + yaw=-90, + angle_units="degrees", + ), + 1e-3, + 1e-3, + True, + ), + ( + Pose( + x=1.0, y=2.0, z=3.0, roll=90, pitch=0.0, yaw=-90, angle_units="degrees" + ), + Pose( + x=1.0 + 1e-4, + y=2.0, + z=3.0, + roll=90, + pitch=1e-4, + yaw=-90, + angle_units="degrees", + ), + 1e-5, + 1e-5, + False, + ), + ], +) +def test_is_approx(pose1, pose2, rel_tol, abs_tol, expected_result): """Test approximate equivalence functionality""" - pose1 = Pose(x=1.0, y=2.0, z=3.0, roll=np.pi / 2, pitch=0.0, yaw=-np.pi / 2) - pose2 = Pose(x=1.0 + 1e-4, y=2.0, z=3.0, roll=np.pi / 2, pitch=1e-4, yaw=-np.pi / 2) # Default values are too tight for the poses above assert not pose1.is_approx(pose2) # Can relax tolerances - assert pose1.is_approx(pose2, rel_tol=1e-3, abs_tol=1e-3) + assert pose1.is_approx(pose2, rel_tol=rel_tol, abs_tol=abs_tol) == expected_result # Check datatype exception. with pytest.raises(TypeError) as exc_info: