Add partial observability to robots (#187)

docs/source/usage/index.rst

@@ -9,6 +9,7 @@ Refer to the following pages for different types of usage guides.
 
    basic_usage
    multirobot
+   robot_actions
    robot_dynamics
    path_planners
    tamp

docs/source/usage/path_planners.rst

@@ -1,5 +1,7 @@
+.. _path_planners:
+
 Path Planners
-=======================
+=============
 This section explains how to use and optionally extend the path planners in ``pyrobosim``.
 
 PathPlanner Interface

docs/source/usage/robot_actions.rst

@@ -0,0 +1,53 @@
+Robot Actions
+=============
+``pyrobosim`` enables you to command your robot to take high-level actions.
+
+The actions currently supported are:
+
+* **Move**: Uses a path planner to navigate to a specific location. Refer to :ref:`path_planners` for more details.
+* **Pick**: Picks up an object at the robot's current location.
+* **Place**: Places an object at the robot's current location.
+* **Detect**: Detects objects at the robot's current location. Refer to :ref:`partial_observability` for more details.
+
+Actions can be triggered in a variety of ways:
+
+* Through the buttons on the GUI.
+* Using a robot's ``execute_action()`` method.
+* (If using ROS), sending a goal to the ``/execute_action`` ROS action server.
+
+You can also command a robot with a *plan*, which is a sequences of actions:
+
+* Using a robot's ``execute_plan()`` method.
+* (If using ROS), sending a goal to the ``/execute_task_plan`` ROS action server.
+
+Plans can also be automatically generated with :ref:`_task_and_motion_planning`.
+
+
+.. _partial_observability:
+
+Partial Observability
+---------------------
+By default, all robots have full knowledge of all the objects in the world.
+
+A common use case for design robot behaviors is that a robot instead starts with limited or no knowledge of objects.
+In these cases, the robot must explicitly go to a location and use an object detector to find new objects to add to their world model.
+
+You can model this in ``pyrobosim`` by instantiating robot objects with the ``partial_observability`` option set to ``True``.
+Then, you can use the **detect** action to find objects at the robot's current location.
+
+To test this, you can run the following example.
+
+::
+
+    cd /path/to/pyrobosim/pyrobosim
+    python3 examples/demo.py --multirobot --partial-observability
+
+In the GUI, selecting a robot in the drop-down menu will only display the objects locally known to that robot.
+Alternatively, you can select the ``world`` option to show all existing objects.
+
+.. image:: ../media/pyrobosim_partial_observability.png
+    :align: center
+    :width: 720px
+    :alt: Partial observability in the pyrobosim GUI.
+
+|

docs/source/usage/tamp.rst

@@ -1,3 +1,5 @@
+.. _task_and_motion_planning:
+
 Task and Motion Planning
 ========================
 We use `PDDLStream <https://github.com/caelan/pddlstream>`_ to perform integrated task and motion planning (TAMP).

docs/source/yaml/world_schema.rst

@@ -46,6 +46,7 @@ The world schema looks as follows, where ``<angle brackets>`` are placeholders:
        grasp_generator:  # For object grasp generation
          type: parallel_grasp  # Supported types -- parallel_grasp
          <property>: <grasp_generator_property>
+       partial_observability: False  # If True, robot starts with no detected objects
      - ...
      - ...
 

pyrobosim/examples/demo.py

@@ -105,6 +105,7 @@ def create_world(multirobot=False):
             linear_velocity=1.0, dt=0.1, max_angular_velocity=4.0
         ),
         grasp_generator=GraspGenerator(grasp_props),
+        partial_observability=args.partial_observability,
     )
     planner_config_rrt = {
         "world": world,
@@ -127,6 +128,7 @@ def create_world(multirobot=False):
             color=(0.8, 0.8, 0),
             path_executor=ConstantVelocityExecutor(),
             grasp_generator=GraspGenerator(grasp_props),
+            partial_observability=args.partial_observability,
         )
         planner_config_prm = {
             "world": world,
@@ -145,6 +147,7 @@ def create_world(multirobot=False):
             color=(0, 0.8, 0.8),
             path_executor=ConstantVelocityExecutor(),
             grasp_generator=GraspGenerator(grasp_props),
+            partial_observability=args.partial_observability,
         )
         planner_config_astar = {
             "grid": OccupancyGrid.from_world(
@@ -179,6 +182,11 @@ def parse_args():
         help="YAML file name (should be in the pyrobosim/data folder). "
         + "If not specified, a world will be created programmatically.",
     )
+    parser.add_argument(
+        "--partial-observability",
+        action="store_true",
+        help="If True, robots have partial observability and must detect objects.",
+    )
     return parser.parse_args()
 
 

pyrobosim/pyrobosim/core/hallway.py

@@ -60,7 +60,7 @@ def __init__(
         if room_end is None:
             raise Exception("room_end must be a valid Room object.")
         if width <= 0.0:
-            raise Exception("width must be a positibe value.")
+            raise Exception("width must be a positive value.")
 
         # Unpack input
         self.room_start = room_start

pyrobosim/pyrobosim/core/robot.py

@@ -31,6 +31,7 @@ def __init__(
         path_planner=None,
         path_executor=None,
         grasp_generator=None,
+        partial_observability=False,
     ):
         """
         Creates a robot instance.
@@ -61,13 +62,19 @@ def __init__(
         :type path_executor: PathExecutor, optional
         :param grasp_generator: Grasp generator for manipulating objects.
         :type grasp_generator: :class:`pyrobosim.manipulation.grasping.GraspGenerator`, optional
+        :param partial_observability: If False, the robot can access all objects in the world.
+            If True, it must detect new objects at specific locations.
+        :type partial_observability: bool, optional
         """
         # Basic properties
         self.name = name
         self.radius = radius
         self.height = height
         self.color = color
 
+        if name == "world":
+            raise ValueError("Robots cannot be named 'world'.")
+
         # Dynamics properties
         self.dynamics = RobotDynamics2D(
             robot=self,
@@ -95,6 +102,9 @@ def __init__(
         self.executing_plan = False
         self.location = None
         self.manipulated_object = None
+        self.partial_observability = partial_observability
+        self.known_objects = set()
+        self.last_detected_objects = []
 
     def get_pose(self):
         """
@@ -172,6 +182,30 @@ def is_in_collision(self):
         """
         return self.dynamics.collision
 
+    def at_object_spawn(self):
+        """
+        Checks whether a robot is at an object spawn.
+
+        :return: True if the robot is at an object spawn, False otherwise.
+        :rtype: bool
+        """
+        return isinstance(self.location, ObjectSpawn)
+
+    def get_known_objects(self):
+        """
+        Returns a list of objects known by the robot.
+
+        :return: The list of known objects.
+        :rtype: list[Object]
+        """
+        if self.world is None:
+            return []
+
+        if self.partial_observability:
+            return list(self.known_objects)
+
+        return self.world.objects
+
     def follow_path(
         self,
         path,
@@ -265,7 +299,7 @@ def pick_object(self, obj_query, grasp_pose=None):
             obj = obj_query
         else:
             obj = self.world.get_object_by_name(obj_query)
-            if not obj:
+            if not obj and isinstance(obj_query, str):
                 obj = resolve_to_object(
                     self.world,
                     category=obj_query,
@@ -385,6 +419,39 @@ def place_object(self, pose=None):
             self.manipulated_object = None
             return True
 
+    def detect_objects(self, target_object=None):
+        """
+        Detects all objects at the robot's current location.
+
+        :param target_object: The name of a target object or category.
+            If None, the action succeeds regardless of which object is found.
+            Otherwise, the action succeeds only if the target object is found.
+        :return: True if detection succeeds, else False
+        :rtype: bool
+        """
+        self.last_detected_objects = []
+
+        if not self.at_object_spawn():
+            warnings.warn(f"Robot is not at an object spawn. Cannot detect objects.")
+            return False
+
+        # Add all the objects at the current robot's location.
+        for obj in self.location.children:
+            self.known_objects.add(obj)
+
+        # If a target object was specified, look for a matching instance.
+        # We should only return True if one such instance was found.
+        if target_object is None:
+            self.last_detected_objects = self.location.children
+            return True
+        else:
+            self.last_detected_objects = [
+                obj
+                for obj in self.location.children
+                if obj.name == target_object or obj.category == target_object
+            ]
+            return len(self.last_detected_objects) > 0
+
     def execute_action(self, action, blocking=False):
         """
         Executes an action, specified as a
@@ -440,6 +507,12 @@ def execute_action(self, action, blocking=False):
             else:
                 success = self.place_object(action.pose)
 
+        elif action.type == "detect":
+            if self.world.has_gui:
+                success = self.world.gui.canvas.detect_objects(self, action.object)
+            else:
+                success = self.detect_objects(action.object)
+
         else:
             print(f"[{self.name}] Invalid action type: {action.type}")
             success = False
@@ -503,4 +576,8 @@ def __repr__(self):
 
     def print_details(self):
         """Prints string with details."""
-        print(f"Robot: {self.name}, ID={self.id}\n\t{self.get_pose()}")
+        details_str = f"Robot: {self.name}, ID={self.id}"
+        details_str += f"\n\t{self.get_pose()}"
+        if self.partial_observability:
+            details_str += "\n\tPartial observability enabled"
+        print(details_str)

pyrobosim/pyrobosim/core/world.py

@@ -545,6 +545,8 @@ def add_object(self, **object_config):
         self.objects.append(obj)
         self.name_to_entity[obj.name] = obj
         self.num_objects += 1
+        if self.has_gui:
+            self.gui.canvas.show_objects()
         return obj
 
     def update_object(self, obj, loc=None, pose=None):
@@ -606,13 +608,17 @@ def remove_object(self, obj):
         """
         if isinstance(obj, str):
             obj = self.get_object_by_name(obj)
-        if obj in self.objects:
-            self.objects.remove(obj)
-            self.name_to_entity.pop(obj.name)
-            self.num_objects -= 1
-            obj.parent.children.remove(obj)
-            return True
-        return False
+
+        if obj not in self.objects:
+            return False
+
+        self.objects.remove(obj)
+        self.name_to_entity.pop(obj.name)
+        self.num_objects -= 1
+        obj.parent.children.remove(obj)
+        if self.has_gui:
+            self.gui.canvas.show_objects()
+        return True
 
     def remove_all_objects(self, restart_numbering=True):
         """

pyrobosim/pyrobosim/core/yaml_utils.py

@@ -128,6 +128,7 @@ def add_robots(self):
                 path_planner=self.get_local_path_planner(robot_data),
                 path_executor=self.get_path_executor(robot_data),
                 grasp_generator=self.get_grasp_generator(robot_data),
+                partial_observability=robot_data.get("partial_observability", False),
             )
 
             loc = robot_data["location"] if "location" in robot_data else None