Enable Open, Close, and Detect actions in PDDL capabilities (#237)

docs/source/usage/tamp.rst

@@ -28,6 +28,7 @@ The current example list is:
 * ``03_nav_stream`` - Samples navigation poses and motion plan instances.
 * ``04_nav_manip_stream`` - Samples navigation poses, motion plans, and collision-free object placement instances.
 * ``05_nav_grasp_stream`` - Samples navigation poses, motion plans, grasp plans, and collision-free object placement instances.
+* ``06_open_close_detect`` - Extends the ``02_derived`` domain with additional actions to detect objects and open and close locations. Does not contain any streams.
 
 These PDDL domain and stream description files can be found in the ``pyrobosim/pyrobosim/data/pddlstream/domains`` folder.
 

pyrobosim/examples/demo_pddl.py

@@ -7,6 +7,7 @@
 import os
 import argparse
 import threading
+import time
 
 from pyrobosim.core import WorldYamlLoader
 from pyrobosim.gui import start_gui
@@ -20,7 +21,7 @@ def parse_args():
     parser.add_argument(
         "--example",
         default="01_simple",
-        help="Example name (01_simple, 02_derived, 03_nav_stream, 04_nav_manip_stream, 05_nav_grasp_stream)",
+        help="Example name (01_simple, 02_derived, 03_nav_stream, 04_nav_manip_stream, 05_nav_grasp_stream, 06_open_close_detect)",
     )
     parser.add_argument("--verbose", action="store_true", help="Print planning output")
     parser.add_argument(
@@ -43,6 +44,10 @@ def start_planner(world, args):
     domain_folder = os.path.join(get_default_domains_folder(), args.example)
     planner = PDDLStreamPlanner(world, domain_folder)
 
+    # Wait for the GUI to load
+    while not world.has_gui:
+        time.sleep(1.0)
+
     if args.example == "01_simple":
         # Task specification for simple example.
         goal_literals = [
@@ -56,6 +61,7 @@ def start_planner(world, args):
         "03_nav_stream",
         "04_nav_manip_stream",
         "05_nav_grasp_stream",
+        "06_open_close_detect",
     ]:
         # Task specification for derived predicate example.
         goal_literals = [
@@ -64,6 +70,9 @@ def start_planner(world, args):
             ("HasNone", "bathroom", "banana"),
             ("HasAll", "table", "water"),
         ]
+        # If using the open/close/detect example, close the desk location.
+        if args.example == "06_open_close_detect":
+            world.close_location(world.get_location_by_name("desk0"))
     else:
         print(f"Invalid example: {args.example}")
         return

pyrobosim/pyrobosim/core/hallway.py

@@ -80,6 +80,7 @@ def __init__(
         self.nav_poses = []
         self.is_open = is_open
         self.is_locked = is_locked
+        self.height = 0.0  # For compatibility with PDDLStream costs
 
         # Parse the connection method
         # If the connection is "auto" or "angle", the hallway is a simple rectangle

pyrobosim/pyrobosim/core/world.py

@@ -499,7 +499,7 @@ def add_location(self, **location_config):
             self.gui.canvas.draw_signal.emit()
         return loc
 
-    def update_location(self, loc, pose, room=None):
+    def update_location(self, loc, pose, room=None, is_open=None, is_locked=None):
         """
         Updates an existing location in the world.
 
@@ -509,6 +509,10 @@ def update_location(self, loc, pose, room=None):
         :type pose: :class:`pyrobosim.utils.pose.Pose`
         :param room: Room instance or name. If none, uses the previous room.
         :type room: :class:`pyrobosim.core.room.Room`/str, optional
+        :param is_open: Whether the location should be open. If None, keeps the current state.
+        :type is_open: bool, optional
+        :param is_locked: Whether the location should be locked. If None, keeps the current state.
+        :type is_locked: bool, optional
         :return: True if the update was successful, else False.
         :rtype: bool
         """
@@ -528,6 +532,11 @@ def update_location(self, loc, pose, room=None):
                 )
                 return False
 
+        if is_open is not None:
+            loc.is_open = is_open
+        if is_locked is not None:
+            loc.is_locked = is_locked
+
         # Check that the location fits within the room and is not in collision with
         # other locations already in the room. Else, warn and do not add it.
         new_polygon = transform_polygon(loc.raw_polygon, pose)

pyrobosim/pyrobosim/data/pddlstream/domains/01_simple/domain.pddl

@@ -15,6 +15,7 @@
     (Robot ?r)          ; Represents the robot
     (Obj ?o)            ; Object representation
     (Room ?r)           ; Room representation
+    (Hallway ?h)        ; Hallway representation
     (Location ?l)       ; Location representation
 
     ; Fluent predicates
@@ -51,6 +52,7 @@
                        (Obj ?o)
                        (Location ?l)
                        (not (Room ?l))
+                       (not (Hallway ?l))
                        (HandEmpty ?r)
                        (At ?r ?l)
                        (At ?o ?l))
@@ -67,6 +69,7 @@
                        (Obj ?o)
                        (Location ?l)
                        (not (Room ?l))
+                       (not (Hallway ?l))
                        (At ?r ?l)
                        (not (HandEmpty ?r))
                        (Holding ?r ?o))

pyrobosim/pyrobosim/data/pddlstream/domains/02_derived/domain.pddl

@@ -18,6 +18,7 @@
     (Robot ?r)              ; Represents the robot
     (Obj ?o)                ; Object representation
     (Room ?r)               ; Room representation
+    (Hallway ?h)            ; Hallway representation
     (Location ?l)           ; Location representation
     (Type ?t)               ; Type of location or object
     (Is ?o ?t)              ; Type correspondence of location or object
@@ -60,6 +61,7 @@
                        (Obj ?o)
                        (Location ?l)
                        (not (Room ?l))
+                       (not (Hallway ?l))
                        (HandEmpty ?r)
                        (At ?r ?l)
                        (At ?o ?l))
@@ -76,6 +78,7 @@
                        (Obj ?o)
                        (Location ?l)
                        (not (Room ?l))
+                       (not (Hallway ?l))
                        (At ?r ?l)
                        (not (HandEmpty ?r))
                        (Holding ?r ?o))

pyrobosim/pyrobosim/data/pddlstream/domains/03_nav_stream/domain.pddl

@@ -18,6 +18,7 @@
     (Robot ?r)              ; Represents the robot
     (Obj ?o)                ; Object representation
     (Room ?r)               ; Room representation
+    (Hallway ?h)            ; Hallway representation
     (Location ?l)           ; Location representation
     (Type ?t)               ; Type of location or object
     (Is ?o ?t)              ; Type correspondence of location or object
@@ -71,6 +72,7 @@
                        (Obj ?o)
                        (Location ?l)
                        (not (Room ?l))
+                       (not (Hallway ?l))
                        (HandEmpty ?r)
                        (At ?r ?l)
                        (At ?o ?l))
@@ -87,6 +89,7 @@
                        (Obj ?o)
                        (Location ?l)
                        (not (Room ?l))
+                       (not (Hallway ?l))
                        (At ?r ?l)
                        (not (HandEmpty ?r))
                        (Holding ?r ?o))

pyrobosim/pyrobosim/data/pddlstream/domains/04_nav_manip_stream/domain.pddl

@@ -21,6 +21,7 @@
     (Robot ?r)              ; Represents the robot
     (Obj ?o)                ; Object representation
     (Room ?r)               ; Room representation
+    (Hallway ?h)            ; Hallway representation
     (Location ?l)           ; Location representation
     (Type ?t)               ; Type of location or object
     (Is ?o ?t)              ; Type correspondence of location or object
@@ -79,6 +80,7 @@
                        (Pose ?p) (AtPose ?o ?p)
                        (Pose ?pr) (AtPose ?r ?pr)
                        (not (Room ?l))
+                       (not (Hallway ?l))
                        (HandEmpty ?r)
                        (At ?r ?l)
                        (At ?o ?l))
@@ -98,6 +100,7 @@
                        (Pose ?p)
                        (Pose ?pr) (AtPose ?r ?pr)
                        (not (Room ?l))
+                       (not (Hallway ?l))
                        (At ?r ?l)
                        (not (HandEmpty ?r))
                        (Holding ?r ?o)

pyrobosim/pyrobosim/data/pddlstream/domains/05_nav_grasp_stream/domain.pddl

@@ -23,6 +23,7 @@
     (Robot ?r)              ; Represents the robot
     (Obj ?o)                ; Object representation
     (Room ?r)               ; Room representation
+    (Hallway ?h)            ; Hallway representation
     (Location ?l)           ; Location representation
     (Type ?t)               ; Type of location or object
     (Is ?o ?t)              ; Type correspondence of location or object
@@ -84,6 +85,7 @@
                        (Pose ?pr) (AtPose ?r ?pr)
                        (Grasp ?g)
                        (not (Room ?l))
+                       (not (Hallway ?l))
                        (HandEmpty ?r)
                        (At ?r ?l)
                        (At ?o ?l)
@@ -104,6 +106,7 @@
                        (Pose ?p)
                        (Pose ?pr) (AtPose ?r ?pr)
                        (not (Room ?l))
+                       (not (Hallway ?l))
                        (At ?r ?l)
                        (not (HandEmpty ?r))
                        (Holding ?r ?o)