Fix bugs in pose setting for locations (#304)

sea-bass · Nov 28, 2024 · 78ef10b · 78ef10b
1 parent ad8c139
commit 78ef10b
Show file tree

Hide file tree

Showing 3 changed files with 82 additions and 23 deletions.
diff --git a/pyrobosim/pyrobosim/core/locations.py b/pyrobosim/pyrobosim/core/locations.py
@@ -139,14 +139,15 @@ def set_pose(self, pose):
             if "offset" in self.metadata["footprint"]:
                 p_off = self.metadata["footprint"]["offset"]
             else:
-                p_off = (0, 0)
+                p_off = (0.0, 0.0)
             for p in self.metadata["nav_poses"]:
-                rot_p = rot2d((p[0] + p_off[0], p[1] + p_off[1]), self.pose.get_yaw())
+                p = Pose.construct(p)
+                rot_p = rot2d((p.x + p_off[0], p.y + p_off[1]), self.pose.get_yaw())
                 nav_pose = Pose(
                     x=rot_p[0] + self.pose.x,
                     y=rot_p[1] + self.pose.y,
                     z=self.pose.z,
-                    yaw=p[2] + self.pose.get_yaw(),
+                    yaw=p.get_yaw() + self.pose.get_yaw(),
                 )
                 if self.parent.is_collision_free(nav_pose):
                     self.nav_poses.append(nav_pose)
@@ -285,16 +286,17 @@ def set_pose_from_parent(self):
             if "offset" in self.metadata["footprint"]:
                 p_off = self.metadata["footprint"]["offset"]
             else:
-                p_off = (0, 0)
+                p_off = (0.0, 0.0)
             for p in self.metadata["nav_poses"]:
+                p = Pose.construct(p)
                 rot_p = rot2d(
-                    (p[0] + p_off[0], p[1] + p_off[1]), self.parent.pose.get_yaw()
+                    (p.x + p_off[0], p.y + p_off[1]), self.parent.pose.get_yaw()
                 )
                 nav_pose = Pose(
                     x=rot_p[0] + self.parent.pose.x,
                     y=rot_p[1] + self.parent.pose.y,
                     z=self.parent.pose.z,
-                    yaw=p[2] + self.parent.pose.get_yaw(),
+                    yaw=p.get_yaw() + self.parent.pose.get_yaw(),
                 )
                 if self.parent.parent.is_collision_free(nav_pose):
                     self.nav_poses.append(nav_pose)

diff --git a/pyrobosim/pyrobosim/core/yaml_utils.py b/pyrobosim/pyrobosim/core/yaml_utils.py
@@ -96,7 +96,8 @@ def add_locations(self):
         """Add locations for object spawning to the world."""
         for loc_data in self.data.get("locations", []):
             loc_args = copy.deepcopy(loc_data)
-            loc_args["pose"] = Pose.construct(loc_args["pose"])
+            if "pose" in loc_args:
+                loc_args["pose"] = Pose.construct(loc_args["pose"])
             self.world.add_location(**loc_args)
 
     def add_objects(self):

diff --git a/pyrobosim/pyrobosim/data/example_location_data.yaml b/pyrobosim/pyrobosim/data/example_location_data.yaml
@@ -8,8 +8,14 @@ table:
     dims: [0.9, 1.2]
     height: 0.5
   nav_poses:
-    - [-0.75, 0, 0]
-    - [0.75, 0, 3.14]
+    - position:  # left
+        x: -0.75
+        y: 0.0
+    - position:  # right
+        x: 0.75
+        y: 0.0
+      rotation_eul:
+        yaw: 3.14
   locations:
     - name: "tabletop"
       footprint:
@@ -31,10 +37,24 @@ desk:
       footprint:
         type: parent
       nav_poses:
-        - [0, -0.5, 1.57]
-        - [-0.5, 0, 0]
-        - [0, 0.5, -1.57]
-        - [0.5, 0, 3.14]
+        - position:  # below
+            x: 0.0
+            y: -0.5
+          rotation_eul:
+            yaw: 1.57
+        - position:  # left
+            x: -0.5
+            y: 0.0
+        - position:  # above
+            x: 0.0
+            y: 0.5
+          rotation_eul:
+            yaw: -1.57
+        - position:  # right
+            x: 0.5
+            y: 0.0
+          rotation_eul:
+            yaw: 3.14
 
 counter:
   footprint:
@@ -52,8 +72,16 @@ counter:
           - [-0.25, 0.25]
         offset: [0.3, 0]
       nav_poses:
-        - [0, 0.5, -1.57]
-        - [0, -0.5, 1.57]
+        - position:  # below
+            x: 0.0
+            y: -0.5
+          rotation_eul:
+            yaw: 1.57
+        - position:  # above
+            x: 0.0
+            y: 0.5
+          rotation_eul:
+            yaw: -1.57
     - name: "right"
       footprint:
         type: polygon
@@ -64,8 +92,16 @@ counter:
           - [-0.25, 0.25]
         offset: [-0.3, 0]
       nav_poses:
-        - [0, 0.5, -1.57]
-        - [0, -0.5, 1.57]
+        - position:  # below
+            x: 0.0
+            y: -0.5
+          rotation_eul:
+            yaw: 1.57
+        - position:  # above
+            x: 0.0
+            y: 0.5
+          rotation_eul:
+            yaw: -1.57
   color: [0, 0.2, 0]
 
 trash_can:
@@ -79,8 +115,14 @@ trash_can:
         type: parent
         padding: 0.05
   nav_poses:
-    - [0.5, 0.0, 3.14]
-    - [-0.5, 0.0, 0.0]
+    - position:  # left
+        x: -0.5
+        y: 0.0
+    - position:  # right
+        x: 0.5
+        y: 0.0
+      rotation_eul:
+        yaw: 3.14
   color: [0, 0.35, 0.2]
 
 charger:
@@ -97,8 +139,22 @@ charger:
       footprint:
         type: parent
       nav_poses:
-        - [0, -0.35, 1.57]
-        - [-0.5, 0, 0]
-        - [0, 0.35, -1.57]
-        - [0.5, 0, 3.14]
+        - position:  # below
+            x: 0.0
+            y: -0.5
+          rotation_eul:
+            yaw: 1.57
+        - position:  # left
+            x: -0.35
+            y: 0.0
+        - position:  # above
+            x: 0.0
+            y: 0.5
+          rotation_eul:
+            yaw: -1.57
+        - position:  # right
+            x: 0.35
+            y: 0.0
+          rotation_eul:
+            yaw: 3.14
   color: [0.4, 0.4, 0]