Allow specifying poses as YAML in world files

docs/source/yaml/world_schema.rst

@@ -23,7 +23,12 @@ The world schema looks as follows, where ``<angle brackets>`` are placeholders:
        radius: <value>  # Robot radius
        height: <value>  # Robot height
        location: <loc_name>  # Initial location
-       pose: [<x>, <y>, <z>, <yaw>]  # Initial pose, if not specified will sample
+       pose:  # Initial pose, if not specified will sample
+         position:
+           x: <x>
+           y: <y>
+         rotation_eul:
+           yaw: <yaw>
        initial_battery_level: 50.0
        # Dynamics limits
        max_linear_velocity: <value>
@@ -89,7 +94,12 @@ The world schema looks as follows, where ``<angle brackets>`` are placeholders:
      - name: <loc_name>  # If not specified, will be automatic
        category: <loc_category>  # From location YAML file
        parent: <room_name>
-       pose: [<x>, <y>, <z>, <yaw>]  # If not specified, will sample
+       pose:  # If not specified, will sample
+        position:
+          x: <x>
+          y: <y>
+        rotation_eul:
+          yaw: <yaw>
        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
@@ -101,4 +111,15 @@ The world schema looks as follows, where ``<angle brackets>`` are placeholders:
      - name: <obj_name>  # If not specified, will be automatic
        category: <obj_category>  # From object YAML file
        parent: <loc_name>
-       pose: [<x>, <y>, <z>, <yaw>]  # If not specified, will sample
+       pose:  # If not specified, will sample
+         position:
+           x: <x>
+           y: <z>
+         rotation_quat:
+           w: <w>
+           x: <x>
+           y: <y>
+           z: <z>
+
+Note that you can use both Euler angles and quaternions to specify poses.
+Any unspecified values will default to ``0.0``.

pyrobosim/pyrobosim/core/yaml_utils.py

@@ -69,10 +69,9 @@ def create_world(self):
     def add_rooms(self):
         """Add rooms to the world."""
         for room_data in self.data.get("rooms", []):
-            # TODO: Find a way to parse poses as YAML.
             if "nav_poses" in room_data:
                 room_data["nav_poses"] = [
-                    Pose.from_list(p) for p in room_data["nav_poses"]
+                    Pose.construct(p) for p in room_data["nav_poses"]
                 ]
 
             self.world.add_room(**room_data)
@@ -85,8 +84,7 @@ def add_hallways(self):
     def add_locations(self):
         """Add locations for object spawning to the world."""
         for loc_data in self.data.get("locations", []):
-            # TODO: Find a way to parse poses as YAML.
-            loc_data["pose"] = Pose.from_list(loc_data["pose"])
+            loc_data["pose"] = Pose.construct(loc_data["pose"])
 
             self.world.add_location(**loc_data)
 
@@ -96,9 +94,8 @@ def add_objects(self):
             return
 
         for obj_data in self.data.get("objects", []):
-            # TODO: Find a way to parse poses as YAML.
             if "pose" in obj_data:
-                obj_data["pose"] = Pose.from_list(obj_data["pose"])
+                obj_data["pose"] = Pose.construct(obj_data["pose"])
             self.world.add_object(**obj_data)
 
     def add_robots(self):
@@ -135,7 +132,7 @@ def add_robots(self):
             if loc:
                 loc = self.world.get_entity_by_name(loc)
             if "pose" in robot_data:
-                pose = Pose.from_list(robot_data["pose"])
+                pose = Pose.construct(robot_data["pose"])
             else:
                 pose = None
             self.world.add_robot(robot, loc=loc, pose=pose)

pyrobosim/pyrobosim/data/pddlstream_simple_world.yaml

@@ -20,7 +20,10 @@ robots:
   - name: robot
     radius: 0.1
     location: kitchen
-    pose: [0, 0, 0]
+    pose:
+      position:
+        x: 0.0
+        y: 0.0
     path_planner:
       type: rrt
       collision_check_step_dist: 0.025
@@ -57,7 +60,9 @@ rooms:
         - [1.5, 1.5]
         - [0.5, 1.5]
     nav_poses:
-      - [0.75, 0.5, 0.0]
+      - position:
+          x: 0.75
+          y: 0.5
     wall_width: 0.2
     color: [1, 0, 0]
 
@@ -115,19 +120,32 @@ hallways:
 locations:
   - category: table
     parent: kitchen
-    pose: [0.85, -0.5, 0.0, -1.57]
+    pose:
+      position:
+        x: 0.85
+        y: -0.5
+      rotation_eul:
+        yaw: -1.57
     is_open: True
     is_locked: True
 
   - category: desk
     parent: bedroom
-    pose: [3.15, 3.65, 0.0, 0.0]
+    pose:
+      position:
+        x: 3.15
+        y: 3.65
     is_open: True
     is_locked: False
 
   - category: counter
     parent: bathroom
-    pose: [-2.45, 2.5, 0.0, 1.767]
+    pose:
+      position:
+        x: -2.45
+        y: 2.5
+      rotation_eul:
+        yaw: 1.767
     is_open: True
     is_locked: True
 
@@ -136,11 +154,19 @@ locations:
 objects:
   - category: banana
     parent: table0
-    pose: [1.0, -0.5, 0.0, 0.707]
+    pose:
+      position:
+        x: 1.0
+        y: -0.5
+      rotation_eul:
+        yaw: 0.707
 
   - category: apple
     parent: desk0
-    pose: [3.2, 3.5, 0.0, 0.0]
+    pose:
+      position:
+        x: 3.2
+        y: 3.5
 
   - category: apple
     parent: table0

pyrobosim/pyrobosim/data/test_world.yaml

@@ -20,7 +20,10 @@ robots:
   - name: robot
     radius: 0.1
     location: kitchen
-    pose: [0, 0, 0]
+    pose:
+      position:
+        x: 0.0
+        y: 0.0
     max_linear_velocity: 1.0
     max_angular_velocity: 4.0
     max_linear_acceleration: 2.5
@@ -67,7 +70,9 @@ rooms:
         - [1.5, 1.5]
         - [0.5, 1.5]
     nav_poses:
-      - [0.75, 0.5, 0.0]
+      - position:
+          x: 0.75
+          y: 0.5
     wall_width: 0.2
     color: [1, 0, 0]
 
@@ -126,28 +131,46 @@ locations:
   - name: table0
     category: table
     parent: kitchen
-    pose: [0.85, -0.5, 0.0, -1.57]
+    pose:
+      position:
+        x: 0.85
+        y: -0.5
+      rotation_eul:
+        yaw: -1.57
     is_open: True
     is_locked: True
 
   - name: my_desk
     category: desk
     parent: bedroom
-    pose: [3.15, 3.65, 0.0, 0.0]
+    pose:
+      position:
+        x: 3.15
+        y: 3.65
     is_open: True
     is_locked: False
 
   - name: counter0
     category: counter
     parent: bathroom
-    pose: [-2.45, 2.5, 0.0, 1.767]
+    pose:
+      position:
+        x: -2.45
+        y: 2.5
+      rotation_eul:
+        yaw: 1.767
     is_open: True
     is_locked: True
 
   - name: trash
     category: trash_can
     parent: kitchen
-    pose: [0.9, 1.1, 0.0, 1.57]
+    pose:
+      position:
+        x: 0.9
+        y: 1.1
+      rotation_eul:
+        yaw: 1.57
     is_open: False
     is_locked: False
 
@@ -156,11 +179,19 @@ locations:
 objects:
   - category: banana
     parent: table0
-    pose: [1.0, -0.5, 0.0, 0.707]
+    pose:
+      position:
+        x: 1.0
+        y: -0.5
+      rotation_eul:
+        yaw: 0.707
 
   - category: apple
     parent: my_desk
-    pose: [3.2, 3.5, 0.0, 0.0]
+    pose:
+      position:
+        x: 3.2
+        y: 3.5
 
   - name: gala
     category: apple

pyrobosim/pyrobosim/data/test_world_multirobot.yaml

@@ -21,7 +21,10 @@ robots:
     radius: 0.1
     color: [0.8, 0.0, 0.8]
     location: kitchen
-    pose: [0, 0, 0]
+    pose:
+      position:
+        x: 0.0
+        y: 0.0
     path_planner:
       type: rrt
       collision_check_step_dist: 0.025
@@ -99,7 +102,9 @@ rooms:
         - [1.5, 1.5]
         - [0.5, 1.5]
     nav_poses:
-      - [0.75, 0.5, 0.0]
+      - position:
+          x: 0.75
+          y: 0.5
     wall_width: 0.2
     color: [1, 0, 0]
 
@@ -158,35 +163,56 @@ locations:
   - name: table0
     category: table
     parent: kitchen
-    pose: [0.85, -0.5, 0.0, -1.57]
+    pose:
+      position:
+        x: 0.85
+        y: -0.5
+      rotation_eul:
+        yaw: -1.57
     is_open: True
     is_locked: True
 
   - name: my_desk
     category: desk
     parent: bedroom
-    pose: [3.15, 3.65, 0.0, 0.0]
+    pose:
+      position:
+        x: 3.15
+        y: 3.65
     is_open: True
     is_locked: False
 
   - name: counter0
     category: counter
     parent: bathroom
-    pose: [-2.45, 2.5, 0.0, 1.767]
+    pose:
+      position:
+        x: -2.45
+        y: 2.5
+      rotation_eul:
+        yaw: 1.767
     is_open: True
     is_locked: True
 
   - name: trash
     category: trash_can
     parent: kitchen
-    pose: [0.9, 1.1, 0.0, 1.57]
+    pose:
+      position:
+        x: 0.9
+        y: 1.1
+      rotation_eul:
+        yaw: 1.57
     is_open: False
     is_locked: False
 
   - name: charger
     category: charger
     parent: bedroom
-    pose: [3.15, 2.7, 0.0, 0.0]
+    pose:
+      position:
+        x: 3.15
+        y: 2.7
     is_open: False
     is_locked: True
     is_charger: True
@@ -196,11 +222,19 @@ locations:
 objects:
   - category: banana
     parent: table0
-    pose: [1.0, -0.5, 0.0, 0.707]
+    pose:
+      position:
+        x: 1.0
+        y: -0.5
+      rotation_eul:
+        yaw: 0.707
 
   - category: apple
     parent: my_desk
-    pose: [3.2, 3.5, 0.0, 0.0]
+    pose:
+      position:
+        x: 3.2
+        y: 3.5
 
   - name: gala
     category: apple