Allow specifying relative poses

docs/source/yaml/world_schema.rst

@@ -69,6 +69,12 @@ The world schema looks as follows, where ``<angle brackets>`` are placeholders:
        footprint:
          type: <footprint_type>
          <property>: <footprint_property>
+       pose:  # If not specified, will use the room's centroid
+         position:
+           x: <x>
+           y: <y>
+         rotation_eul:
+           yaw: <yaw>
        nav_poses:
          - [<x1>, <y1>, <z1>, <yaw1>]
          - ...
@@ -102,6 +108,7 @@ The world schema looks as follows, where ``<angle brackets>`` are placeholders:
         rotation_eul:
           yaw: <yaw>
           angle_units: <units>  # Can be "radians" (default) or "degrees"
+        relative_to: <room_name>  # If not specified, uses absolute pose
        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,7 +129,71 @@ The world schema looks as follows, where ``<angle brackets>`` are placeholders:
            x: <x>
            y: <y>
            z: <z>
+         relative_to: <loc_name>  # If not specified, uses absolute pose
+
+
+Specifying Poses
+----------------
+
+There are a few ways to specify poses in PyRoboSim YAML files: lists and dictionaries.
+
+.. code-block:: yaml
+
+   # Valid list formats
+   pose: [<x>, <y>]
+   pose: [<x>, <y>, <z>]
+   pose: [<x>, <y>, <z>, <yaw>]  # Angle units always in radians
+
+If possible, you should use the dictionary format, as the list format is at this point only around for backward compatibility.
+Anything below this line is only supported in dictionary format.
 
 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``.
+
+.. code-block:: yaml
+
+   # Euler angles in radians (default), fully specified
+   pose:
+     position:
+       x: 1.0
+       y: 2.0
+       z: 3.0
+     rotation_eul:
+       yaw: 0.1
+       pitch: 0.2
+       roll: 0.3
+
+   # Euler angles in degrees, partially specified
+   pose:
+     position:
+       x: 1.0
+       y: 2.0
+     rotation_eul:
+       yaw: 45.0
+       angle_units: "degrees"
+
+   # Quaternion
+   pose:
+     position:
+       x: 1.0
+       y: 2.0
+     rotation_quat:
+       w: 0.707
+       x: 0.0
+       y: 0.0
+       z: -0.707
+
+You can also use the ``relative_to`` field when specifying poses.
+This makes it easier to specify poses relative to other entities in the world (rooms, locations, objects, etc.).
+
+.. code-block:: yaml
+
+   pose:
+     position:
+       x: 1.0
+       y: 2.0
+     rotation_eul:
+       yaw: 45.0
+       angle_units: "degrees"
+     relative_to: "table0"

pyrobosim/examples/demo.py

@@ -5,7 +5,6 @@
 """
 import os
 import argparse
-import numpy as np
 
 from pyrobosim.core import Robot, World, WorldYamlLoader
 from pyrobosim.gui import start_gui
@@ -37,12 +36,18 @@ def create_world(multirobot=False):
     r1coords = [(-1, -1), (1.5, -1), (1.5, 1.5), (0.5, 1.5)]
     world.add_room(
         name="kitchen",
+        pose=Pose(x=0.0, y=0.0, z=0.0, yaw=0.0),
         footprint=r1coords,
         color="red",
         nav_poses=[Pose(x=0.75, y=0.75, z=0.0, yaw=0.0)],
     )
-    r2coords = [(1.75, 2.5), (3.5, 2.5), (3.5, 4), (1.75, 4)]
-    world.add_room(name="bedroom", footprint=r2coords, color="#009900")
+    r2coords = [(-0.875, -0.75), (0.875, -0.75), (0.875, 0.75), (-0.875, 0.75)]
+    world.add_room(
+        name="bedroom",
+        pose=Pose(x=2.625, y=3.25, z=0.0, yaw=0.0),
+        footprint=r2coords,
+        color="#009900",
+    )
     r3coords = [(-1, 1), (-1, 3.5), (-3.0, 3.5), (-2.5, 1)]
     world.add_room(
         name="bathroom",
@@ -77,24 +82,25 @@ def create_world(multirobot=False):
         parent="kitchen",
         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)
+    desk_pose = world.get_pose_relative_to(
+        Pose(x=0.525, y=0.4, z=0.0, yaw=0.0), "bedroom"
     )
+    desk = world.add_location(category="desk", parent="bedroom", pose=desk_pose)
     counter = world.add_location(
         category="counter",
         parent="bathroom",
         pose=Pose(x=-2.45, y=2.5, z=0.0, q=[0.634411, 0.0, 0.0, 0.7729959]),
     )
 
     # Add objects
-    world.add_object(
-        category="banana",
-        parent=table,
-        pose=Pose(x=1.0, y=-0.5, z=0.0, q=[0.9238811, 0.0, 0.0, 0.3826797]),
+    banana_pose = world.get_pose_relative_to(
+        Pose(x=0.15, y=0.0, z=0.0, q=[0.9238811, 0.0, 0.0, -0.3826797]), table
     )
-    world.add_object(
-        category="apple", parent=desk, pose=Pose(x=3.2, y=3.5, z=0.0, yaw=0.0)
+    world.add_object(category="banana", parent=table, pose=banana_pose)
+    apple_pose = world.get_pose_relative_to(
+        Pose(x=0.05, y=-0.15, z=0.0, q=[1.0, 0.0, 0.0, 0.0]), desk
     )
+    world.add_object(category="apple", parent=desk, pose=apple_pose)
     world.add_object(category="apple", parent=table)
     world.add_object(category="apple", parent=table)
     world.add_object(category="water", parent=counter)

pyrobosim/pyrobosim/core/robot.py

@@ -225,9 +225,7 @@ def at_openable_location(self):
         :return: True if the robot is at an openable location, else False.
         :type: bool
         """
-        return isinstance(self.location, Hallway) or isinstance(
-            self.location, ObjectSpawn
-        )
+        return isinstance(self.location, (Hallway, ObjectSpawn))
 
     def _attach_object(self, obj):
         """

pyrobosim/pyrobosim/core/room.py

@@ -6,10 +6,12 @@
 from shapely.geometry import Polygon
 from shapely.plotting import patch_from_polygon
 
-from matplotlib.colors import CSS4_COLORS, to_rgb
-
 from ..utils.pose import Pose
-from ..utils.polygon import inflate_polygon, polygon_and_height_from_footprint
+from ..utils.polygon import (
+    inflate_polygon,
+    polygon_and_height_from_footprint,
+    transform_polygon,
+)
 from ..utils.search_graph import Node
 from ..utils.general import parse_color
 
@@ -23,6 +25,7 @@ def __init__(
         footprint=[],
         color=[0.4, 0.4, 0.4],
         wall_width=0.2,
+        pose=None,
         nav_poses=None,
         height=0.0,
     ):
@@ -42,6 +45,9 @@ def __init__(
         :type color: list[float] | tuple[float, float, float] | str
         :param wall_width: Width of room walls, in meters.
         :type wall_width: float, optional
+        :param pose: Pose of the room. This transforms the specified footprint.
+            If set to None, the pose will be the centroid of the room polygon.
+        :type pose: :class:`pyrobosim.utils.pose.Pose`, optional
         :param nav_poses: List of navigation poses in the room. If not specified, defaults to the centroid.
         :type nav_poses: list[:class:`pyrobosim.utils.pose.Pose`]
         :param height: Height of room.
@@ -69,15 +75,24 @@ def __init__(
         if self.polygon.is_empty:
             raise Exception("Room footprint cannot be empty.")
 
+        self.original_pose = pose  # Needed to serialize the world properly.
+        if pose is not None:
+            self.pose = pose
+            self.polygon = transform_polygon(self.polygon, self.pose)
+
         self.centroid = list(self.polygon.centroid.coords)[0]
+        centroid_pose = Pose.from_list(self.centroid)
+        if pose is None:
+            self.pose = centroid_pose
+
         self.update_collision_polygons()
         self.update_visualization_polygon()
 
-        # Create a navigation pose list -- if none specified, use the room centroid
+        # Create a navigation pose list -- if none specified, use the room centroid.
         if nav_poses is not None:
             self.nav_poses = nav_poses
         else:
-            self.nav_poses = [Pose.from_list(self.centroid)]
+            self.nav_poses = [centroid_pose]
 
     def update_collision_polygons(self, inflation_radius=0):
         """
@@ -157,14 +172,17 @@ def to_dict(self):
         :return: A dictionary containing the room information.
         :rtype: dict[str, Any]
         """
-        return {
+        room_dict = {
             "name": self.name,
             "color": self.viz_color,
             "wall_width": self.wall_width,
             "footprint": self.footprint,
             "height": self.height,
             "nav_poses": [pose.to_dict() for pose in self.nav_poses],
         }
+        if self.original_pose is not None:
+            room_dict["pose"] = self.original_pose.to_dict()
+        return room_dict
 
     def __repr__(self):
         """Returns printable string."""

pyrobosim/pyrobosim/core/world.py

@@ -530,7 +530,7 @@ def open_location(self, location):
         # Validate the input
         if isinstance(location, str):
             location = self.get_entity_by_name(location)
-        if not (isinstance(location, Location) or isinstance(location, Hallway)):
+        if not isinstance(location, (Location, Hallway)):
             message = message = (
                 f"Cannot open {location} since it is of type {type(location).__name__}."
             )
@@ -582,7 +582,7 @@ def close_location(self, location, ignore_robots=[]):
         # Validate the input
         if isinstance(location, str):
             location = self.get_entity_by_name(location)
-        if not (isinstance(location, Location) or isinstance(location, Hallway)):
+        if not isinstance(location, (Location, Hallway)):
             message = message = (
                 f"Cannot close {location} since it is of type {type(location).__name__}."
             )
@@ -642,7 +642,7 @@ def lock_location(self, location):
         # Validate the input
         if isinstance(location, str):
             location = self.get_entity_by_name(location)
-        if not (isinstance(location, Location) or isinstance(location, Hallway)):
+        if not isinstance(location, (Location, Hallway)):
             message = message = (
                 f"Cannot lock {location} since it is of type {type(location).__name__}."
             )
@@ -678,7 +678,7 @@ def unlock_location(self, location):
         # Validate the input
         if isinstance(location, str):
             location = self.get_entity_by_name(location)
-        if not (isinstance(location, Location) or isinstance(location, Hallway)):
+        if not isinstance(location, (Location, Hallway)):
             message = message = (
                 f"Cannot unlock {location} since it is of type {type(location).__name__}."
             )
@@ -984,7 +984,7 @@ def add_robot(self, robot, loc=None, pose=None):
             if isinstance(loc, str):
                 loc = self.get_entity_by_name(loc)
 
-            if isinstance(loc, Room) or isinstance(loc, Hallway):
+            if isinstance(loc, (Room, Hallway)):
                 if pose is None:
                     # Sample a pose in the location
                     x_sample, y_sample = sample_from_polygon(
@@ -1002,7 +1002,7 @@ def add_robot(self, robot, loc=None, pose=None):
                         self.logger.warning(f"{pose} is occupied")
                         valid_pose = False
                     robot_pose = pose
-            elif isinstance(loc, Location) or isinstance(loc, ObjectSpawn):
+            elif isinstance(loc, (Location, ObjectSpawn)):
                 if isinstance(loc, Location):
                     # NOTE: If you don't want a random object spawn, use the object spawn as the input location.
                     loc = np.random.choice(loc.children)
@@ -1497,10 +1497,39 @@ def get_entity_by_name(self, name):
         :type name: str
         :return: Entity object instance matching the input name, or ``None`` if not valid.
         """
-        if name in self.name_to_entity:
-            return self.name_to_entity[name]
-        else:
-            return None
+        return self.name_to_entity.get(name)
+
+    def get_pose_relative_to(self, pose, entity):
+        """
+        Given a relative pose to an entity, and the entity itself, gets the absolute pose.
+
+        :param pose: The pose specified with respect to the entity.
+        :type pose: :class:`pyrobosim.utils.pose.Pose`
+        :param entity: The entity, or entity name:
+        :type entity: Entity or str
+        :return: Absolute pose computed by transforming the input pose to the entity frame.
+        :rtype: :class:`pyrobosim.utils.pose.Pose`
+        """
+        # Look for the target entity if specified by name.
+        if isinstance(entity, str):
+            grounded_entity = self.get_entity_by_name(entity)
+            if grounded_entity is None:
+                raise ValueError(f"Could not find entity by name: {entity}")
+
+            entity = grounded_entity
+
+        # Validate that the entity is of one of the supported types.
+        if not isinstance(entity, (Room, Location, ObjectSpawn, Object, Robot)):
+            raise TypeError(f"Invalid entity type: {type(entity)}")
+
+        # Extract the pose from the entity and use it to transform the given pose
+        # to the entity frame.
+        # Note that the rotation is applied separately from the translation.
+        new_transform = (
+            pose.get_translation_matrix() @ entity.pose.get_transform_matrix()
+        )
+        new_transform[:3, :3] = pose.get_rotation_matrix() @ new_transform[:3, :3]
+        return Pose.from_transform(new_transform)
 
     #######################
     # Occupancy utilities #
@@ -1666,7 +1695,7 @@ def graph_node_from_entity(
         else:
             entity = entity_query
 
-        if isinstance(entity, ObjectSpawn) or isinstance(entity, Room):
+        if isinstance(entity, (ObjectSpawn, Room)):
             graph_nodes = entity.graph_nodes
         elif isinstance(entity, Hallway):
             graph_nodes = [entity.graph_nodes[0], entity.graph_nodes[-1]]