diff --git a/docs/source/yaml/location_schema.rst b/docs/source/yaml/location_schema.rst index b9c8662d..90dbb690 100644 --- a/docs/source/yaml/location_schema.rst +++ b/docs/source/yaml/location_schema.rst @@ -12,9 +12,28 @@ The generic location schema, where ```` are placeholders, is: type: : nav_poses: - - [, , , ] - - ... - - [, , , ] + - position: + x: + y: + rotation_eul: + yaw: + angle_units: "degrees" + - position: + x: + y: + rotation_quat: + w: + x: + y: + z: + - ... + - position: + x: + y: + z: + rotation_eul: + yaw: + angle_units: locations: - name: footprint: @@ -25,7 +44,7 @@ The generic location schema, where ```` are placeholders, is: footprint: type: : - color: [, , ] + color: [, , ] or <"color_name"> or <"hexadecimalcode"> is_open: True is_locked: False is_charger: False @@ -87,7 +106,7 @@ A more complex object with a box footprint and two separate object spawns. nav_poses: - [0, 0.5, 0.0, -1.57] - [0, -0.5, 0.0, 1.57] - color: [0, 0.2, 0] # Dark green + color: "#003300" # Dark green A location with a footprint read from a mesh file. Note that the literal ``$DATA`` resolves to the ``pyrobosim/data`` folder, but you can specify an absolute path as well or create your own tokens. diff --git a/docs/source/yaml/object_schema.rst b/docs/source/yaml/object_schema.rst index e27a0a0b..d7c66794 100644 --- a/docs/source/yaml/object_schema.rst +++ b/docs/source/yaml/object_schema.rst @@ -11,7 +11,7 @@ The generic object schema, where ```` are placeholders, is: footprint: type: : - color: [, , ] + color: [, , ] or <"color_name"> or <"hexadecimalcode"> Examples -------- @@ -35,7 +35,7 @@ A simple object with a box footprint. type: box # Box footprint dims: [0.05, 0.2] # 5 cm by 20 cm offset: [0.0, 0.1] # 10 cm Y offset from origin - color: [0.7, 0.7, 0] # Yellow + color: "yellow" # Yellow An object with a generic polygon footprint. @@ -64,4 +64,4 @@ Note that the literal ``$DATA`` resolves to the ``pyrobosim/data`` folder, but y type: mesh # Geometry from mesh model_path: $DATA/sample_models/coke_can mesh_path: meshes/coke_can.dae - color: [0.8, 0, 0] # Red color (for viewing in pyrobosim) + color: "#CC0000" # Red color (for viewing in pyrobosim) diff --git a/docs/source/yaml/world_schema.rst b/docs/source/yaml/world_schema.rst index 3e47604e..17996b2a 100644 --- a/docs/source/yaml/world_schema.rst +++ b/docs/source/yaml/world_schema.rst @@ -29,6 +29,7 @@ The world schema looks as follows, where ```` are placeholders: y: rotation_eul: yaw: + angle_units: # Can be "radians" (default) or "degrees" initial_battery_level: 50.0 # Dynamics limits max_linear_velocity: @@ -73,7 +74,7 @@ The world schema looks as follows, where ```` are placeholders: - ... - [, , , ] wall_width: - color: [, , ] + color: [, , ] or <"color_name"> or <"hexadecimalcode"> - ... - ... @@ -100,6 +101,7 @@ The world schema looks as follows, where ```` are placeholders: y: rotation_eul: yaw: + angle_units: # Can be "radians" (default) or "degrees" is_open: true # Can only pick, place, and detect if open is_locked: true # Can only open and close if unlocked is_charger: false # Robots can charge at this location @@ -122,4 +124,5 @@ The world schema looks as follows, where ```` are placeholders: z: Note that you can use both Euler angles and quaternions to specify poses. +If specifying rotation using Euler angles, you can specify angle either in radians or degrees. Any unspecified values will default to ``0.0``. diff --git a/pyrobosim/examples/demo.py b/pyrobosim/examples/demo.py index d10efc52..4924b519 100755 --- a/pyrobosim/examples/demo.py +++ b/pyrobosim/examples/demo.py @@ -75,7 +75,7 @@ def create_world(multirobot=False): table = world.add_location( category="table", parent="kitchen", - pose=Pose(x=0.85, y=-0.5, z=0.0, yaw=-np.pi / 2.0), + pose=Pose(x=0.85, y=-0.5, z=0.0, yaw=-90.0, angle_units="degrees"), ) desk = world.add_location( category="desk", parent="bedroom", pose=Pose(x=3.15, y=3.65, z=0.0, yaw=0.0) diff --git a/pyrobosim/pyrobosim/data/pddlstream_simple_world.yaml b/pyrobosim/pyrobosim/data/pddlstream_simple_world.yaml index 00c5b057..2565d176 100644 --- a/pyrobosim/pyrobosim/data/pddlstream_simple_world.yaml +++ b/pyrobosim/pyrobosim/data/pddlstream_simple_world.yaml @@ -125,7 +125,8 @@ locations: x: 0.85 y: -0.5 rotation_eul: - yaw: -1.57 + yaw: -90.0 + angle_units: "degrees" is_open: True is_locked: True @@ -145,7 +146,8 @@ locations: x: -2.45 y: 2.5 rotation_eul: - yaw: 1.767 + yaw: 101.2 + angle_units: "degrees" is_open: True is_locked: True @@ -159,7 +161,8 @@ objects: x: 1.0 y: -0.5 rotation_eul: - yaw: 0.707 + yaw: 40.5 + angle_units: "degrees" - category: apple parent: desk0 diff --git a/pyrobosim/pyrobosim/data/test_world.yaml b/pyrobosim/pyrobosim/data/test_world.yaml index e0b0e8e2..54b40039 100644 --- a/pyrobosim/pyrobosim/data/test_world.yaml +++ b/pyrobosim/pyrobosim/data/test_world.yaml @@ -139,7 +139,8 @@ locations: x: 0.85 y: -0.5 rotation_eul: - yaw: -1.57 + yaw: -90.0 + angle_units: "degrees" is_open: True is_locked: True @@ -161,7 +162,8 @@ locations: x: -2.45 y: 2.5 rotation_eul: - yaw: 1.767 + yaw: 101.2 + angle_units: "degrees" is_open: True is_locked: True @@ -173,7 +175,9 @@ locations: x: 0.9 y: 1.1 rotation_eul: - yaw: 1.57 + yaw: 90.0 + angle_units: "degrees" + is_open: False is_locked: False @@ -187,7 +191,8 @@ objects: x: 1.0 y: -0.5 rotation_eul: - yaw: 0.707 + yaw: 40.5 + angle_units: "degrees" - category: apple parent: my_desk diff --git a/pyrobosim/pyrobosim/data/test_world_multirobot.yaml b/pyrobosim/pyrobosim/data/test_world_multirobot.yaml index 78097e8f..448aadd1 100644 --- a/pyrobosim/pyrobosim/data/test_world_multirobot.yaml +++ b/pyrobosim/pyrobosim/data/test_world_multirobot.yaml @@ -170,7 +170,8 @@ locations: x: 0.85 y: -0.5 rotation_eul: - yaw: -1.57 + yaw: -90.0 + angle_units: "degrees" is_open: True is_locked: True @@ -192,7 +193,8 @@ locations: x: -2.45 y: 2.5 rotation_eul: - yaw: 1.767 + yaw: 101.2 + angle_units: "degrees" is_open: True is_locked: True @@ -204,7 +206,8 @@ locations: x: 0.9 y: 1.1 rotation_eul: - yaw: 1.57 + yaw: 90.0 + angle_units: "degrees" is_open: False is_locked: False @@ -229,7 +232,8 @@ objects: x: 1.0 y: -0.5 rotation_eul: - yaw: 0.707 + yaw: 40.5 + angle_units: "degrees" - category: apple parent: my_desk diff --git a/pyrobosim/pyrobosim/utils/pose.py b/pyrobosim/pyrobosim/utils/pose.py index ee6f992b..be197726 100644 --- a/pyrobosim/pyrobosim/utils/pose.py +++ b/pyrobosim/pyrobosim/utils/pose.py @@ -11,7 +11,17 @@ class Pose: """Represents a 3D pose.""" - def __init__(self, x=0.0, y=0.0, z=0.0, roll=0.0, pitch=0.0, yaw=0.0, q=None): + def __init__( + self, + x=0.0, + y=0.0, + z=0.0, + roll=0.0, + pitch=0.0, + yaw=0.0, + q=None, + angle_units="radians", + ): """ Creates a new Pose object. @@ -21,12 +31,14 @@ def __init__(self, x=0.0, y=0.0, z=0.0, roll=0.0, pitch=0.0, yaw=0.0, q=None): :type y: float :param z: Z position :type z: float - :param roll: Roll angle (about X axis), in radians + :param roll: Roll angle (about X axis), in specified angle units :type roll: float - :param pitch: Pitch angle (about Y axis), in radians + :param pitch: Pitch angle (about Y axis), in specified angle units :type pitch: float - :param yaw: Yaw angle (about Z axis), in radians + :param yaw: Yaw angle (about Z axis), in specified angle units :type yaw: float + :param angle_units: Units for angle ('radians' or 'degrees'). Default is 'radians' + :type angle_units: str :param q: Quaternion, specified at [qw, qx, qy, qz] If specified, will override roll/pitch/yaw values. :type q: list[float] @@ -37,8 +49,18 @@ def __init__(self, x=0.0, y=0.0, z=0.0, roll=0.0, pitch=0.0, yaw=0.0, q=None): if q is not None: self.set_quaternion(q) - else: - self.set_euler_angles(roll, pitch, yaw) + return + + if angle_units not in ("radians", "degrees"): + raise ValueError( + "Invalid angle units provided. It should be either 'radians' or 'degrees'." + "Additionally, if 'q' is provided, angle units are ignored." + ) + + if angle_units == "degrees": + roll, pitch, yaw = map(math.radians, [roll, pitch, yaw]) + + self.set_euler_angles(roll, pitch, yaw) @classmethod def from_list(cls, plist): @@ -51,6 +73,7 @@ def from_list(cls, plist): * 6-element lists: ``[x, y, z, roll, pitch, yaw]`` * 7-element lists: ``[x, y, z, qw, qx, qy, qz]`` * other lengths: invalid + * angle units are always "radians" :param plist: List containing the input pose (see format above). :type plist: list[float] @@ -64,7 +87,12 @@ def from_list(cls, plist): elif num_elems == 3: return cls(x=plist[0], y=plist[1], z=plist[2]) elif num_elems == 4: - return cls(x=plist[0], y=plist[1], z=plist[2], yaw=plist[3]) + return cls( + x=plist[0], + y=plist[1], + z=plist[2], + yaw=plist[3], + ) elif num_elems == 6: return cls( x=plist[0], @@ -110,6 +138,7 @@ def from_dict(cls, pose_dict): yaw: 0.5 pitch: 0.6 roll: 0.7 + angle_units: "radians" rotation_quat: w: 0.7071 x: 0.0 @@ -125,7 +154,11 @@ def from_dict(cls, pose_dict): """ pos = pose_dict.get("position", {}) - args = {"x": pos.get("x", 0.0), "y": pos.get("y", 0.0), "z": pos.get("z", 0.0)} + args = { + "x": pos.get("x", 0.0), + "y": pos.get("y", 0.0), + "z": pos.get("z", 0.0), + } quat = pose_dict.get("rotation_quat") eul = pose_dict.get("rotation_eul") @@ -146,6 +179,7 @@ def from_dict(cls, pose_dict): "yaw": eul.get("yaw", 0.0), "pitch": eul.get("pitch", 0.0), "roll": eul.get("roll", 0.0), + "angle_units": eul.get("angle_units", "radians"), } ) return cls(**args) diff --git a/pyrobosim/test/utils/test_pose_utils.py b/pyrobosim/test/utils/test_pose_utils.py index 087205a8..7ea55d55 100644 --- a/pyrobosim/test/utils/test_pose_utils.py +++ b/pyrobosim/test/utils/test_pose_utils.py @@ -40,9 +40,17 @@ 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", + [ + ("radians", np.pi / 2, 0.0, -np.pi / 2), + ("degrees", 90.0, 0.0, -90.0), + ], +) +def test_pose_from_euler_angles(angle_units, roll, pitch, yaw): + """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) @@ -102,6 +110,14 @@ 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] == ( + "Invalid angle units provided. It should be either 'radians' or 'degrees'." + "Additionally, if 'q' is provided, angle units are ignored." + ) + def test_pose_from_transform(): """Test creating a pose using a transform.""" @@ -122,7 +138,10 @@ def test_pose_from_transform(): assert pose.q == pytest.approx([0.5, 0.5, 0.5, -0.5]) -def test_pose_to_from_dict(): +@pytest.mark.parametrize( + "angle_units, angle_value", [("radians", np.pi / 2), ("degrees", 90.0)] +) +def test_pose_to_from_dict(angle_units, angle_value): """Test creating poses using a dictionary and saving them back out.""" pose_dict = {} pose = Pose.from_dict(pose_dict) @@ -135,31 +154,24 @@ def test_pose_to_from_dict(): pose_dict = { "position": {"x": 1.0, "y": 2.0, "z": 3.0}, "rotation_eul": { - "yaw": np.pi / 2.0, - "pitch": np.pi / 4.0, - "roll": -np.pi / 2.0, + "yaw": angle_value, + "pitch": angle_value / 2, + "roll": -angle_value, + "angle_units": angle_units, }, } pose = Pose.from_dict(pose_dict) 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.eul == pytest.approx([-np.pi / 2, np.pi / 4, np.pi / 2]) - pose_dict = { - "position": {"x": 1.0, "y": 2.0, "z": 3.0}, - "rotation_eul": { - "yaw": np.pi / 2.0, - "pitch": np.pi / 4.0, - "roll": -np.pi / 2.0, - }, - "rotation_quat": {"w": 0.707107, "x": -0.707107, "y": 0.0, "z": 0.0}, - } + pose_dict["rotation_quat"] = {"w": 0.707107, "x": -0.707107, "y": 0.0, "z": 0.0} pose = Pose.from_dict(pose_dict) 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.eul == pytest.approx([-np.pi / 2, 0.0, 0.0]) assert pose.q == pytest.approx([0.707107, -0.707107, 0.0, 0.0]) out_dict = pose.to_dict() @@ -183,18 +195,20 @@ def test_pose_to_from_dict(): assert quat["z"] == pytest.approx(0.0) -def test_construct_pose(): - """Test pose construct function that accepts various types.""" - pose_from_list = Pose.construct([1.0, 2.0, 3.0, np.pi / 2.0]) - +@pytest.mark.parametrize( + "angle_units, angle_value", [("radians", np.pi / 2), ("degrees", 90.0)] +) +def test_construct_pose(angle_units, angle_value): + """Test pose construct function that accepts various types""" + pose_from_list = Pose.construct([1.0, 2.0, 3.0, np.pi / 2]) pose_from_dict = Pose.construct( { "position": {"x": 1.0, "y": 2.0, "z": 3.0}, - "rotation_eul": {"yaw": np.pi / 2.0}, + "rotation_eul": {"yaw": angle_value, "angle_units": angle_units}, } ) - tform = np.array( + expected_tform = np.array( [ [0.0, -1.0, 0.0, 1.0], [1.0, 0.0, 0.0, 2.0], @@ -202,7 +216,8 @@ def test_construct_pose(): [0.0, 0.0, 0.0, 1.0], ] ) - pose_from_tform = Pose.construct(tform) + + pose_from_tform = Pose.construct(expected_tform) assert pose_from_list.is_approx(pose_from_dict) assert pose_from_dict.is_approx(pose_from_tform) @@ -239,9 +254,24 @@ 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) +@pytest.mark.parametrize( + "x, y, z, roll, pitch, yaw, angle_units", + [ + (1.0, 2.0, 3.0, np.pi / 2, 0.0, -np.pi / 2, "radians"), + (1.0, 2.0, 3.0, 90.0, 0.0, -90.0, "degrees"), + ], +) +def test_get_matrices(x, y, z, roll, pitch, yaw, angle_units): + """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_tform = np.array( [ @@ -251,6 +281,7 @@ def test_get_matrices(): [0.0, 0.0, 0.0, 1.0], ] ) + expected_translation_matrix = np.eye(4) expected_translation_matrix[:3, 3] = expected_tform[:3, 3] @@ -259,10 +290,26 @@ def test_get_matrices(): assert pose.get_translation_matrix() == pytest.approx(expected_translation_matrix) -def test_is_approx(): - """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) +def test_is_approx_radians(): + """Test approximate equivalence functionality for pose in radians""" + pose1 = Pose( + x=1.0, + y=2.0, + z=3.0, + roll=np.pi / 2, + pitch=0.0, + yaw=-np.pi / 2, + angle_units="radians", + ) + pose2 = 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", + ) # Default values are too tight for the poses above assert not pose1.is_approx(pose2) @@ -276,6 +323,28 @@ def test_is_approx(): assert exc_info.value.args[0] == "Expected a Pose object." +def test_is_approx_degrees(): + """Test approximate equivalence functionality for pose in degrees""" + pose1 = Pose( + x=1.0, y=2.0, z=3.0, roll=90.0, pitch=0.0, yaw=-90.0, angle_units="degrees" + ) + pose2 = Pose( + x=1.0 + 1e-4, + y=2.0, + z=3.0, + roll=90.0, + pitch=0.0057, + yaw=-90.0, + angle_units="degrees", + ) + + # 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) + + def test_equality(): """Test pose equality operator.""" # Original pose