Add support to allow pose angles in degrees (#309)

docs/source/yaml/location_schema.rst

@@ -12,9 +12,28 @@ The generic location schema, where ``<angle brackets>`` are placeholders, is:
        type: <footprint_type>
        <property>: <footprint_property>
      nav_poses:
-       - [<x1>, <y1>, <z1>, <yaw1>]
-       - ...
-       - [<xN>, <yN>, <zN>, <yawN>]
+       - position:
+           x: <x1>
+           y: <y1>
+         rotation_eul:
+           yaw: <yaw1>
+           angle_units: "degrees"
+       - position:
+           x: <x2>
+           y: <y2>
+         rotation_quat:
+           w: <w2>
+           x: <x2>
+           y: <y2>
+           z: <z2>
+       -  ...
+       - position:
+           x: <xN>
+           y: <yN>
+           z: <zN>
+         rotation_eul:
+           yaw: <yawN>
+           angle_units: <units>
      locations:
        - name: <location_name>
          footprint:
@@ -25,7 +44,7 @@ The generic location schema, where ``<angle brackets>`` are placeholders, is:
          footprint:
            type: <footprint_type>
            <property>: <footprint_property>
-     color: [<r>, <g>, <b>]
+     color: [<r>, <g>, <b>] 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.

docs/source/yaml/object_schema.rst

@@ -11,7 +11,7 @@ The generic object schema, where ``<angle brackets>`` are placeholders, is:
      footprint:
        type: <footprint_type>
        <property>: <footprint_property>
-     color: [<r>, <g>, <b>]
+     color: [<r>, <g>, <b>] 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)

docs/source/yaml/world_schema.rst

@@ -29,6 +29,7 @@ The world schema looks as follows, where ``<angle brackets>`` are placeholders:
            y: <y>
          rotation_eul:
            yaw: <yaw>
+           angle_units: <units>  # Can be "radians" (default) or "degrees"
        initial_battery_level: 50.0
        # Dynamics limits
        max_linear_velocity: <value>
@@ -73,7 +74,7 @@ The world schema looks as follows, where ``<angle brackets>`` are placeholders:
          - ...
          - [<xN>, <yN>, <zN>, <yawN>]
        wall_width: <value>
-       color: [<r>, <g>, <b>]
+       color: [<r>, <g>, <b>] or <"color_name"> or <"hexadecimalcode">
      - ...
      - ...
 
@@ -100,6 +101,7 @@ The world schema looks as follows, where ``<angle brackets>`` are placeholders:
           y: <y>
         rotation_eul:
           yaw: <yaw>
+          angle_units: <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 ``<angle brackets>`` are placeholders:
            z: <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``.

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)

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

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

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

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)