From 72611b01756b95e9119167ca45fd554fbb54bb2a Mon Sep 17 00:00:00 2001 From: Sebastian Castro <4603398+sea-bass@users.noreply.github.com> Date: Tue, 13 Aug 2024 21:10:55 -0400 Subject: [PATCH] Enable Open, Close, and Detect actions in PDDL capabilities (#237) --- docs/source/usage/tamp.rst | 1 + pyrobosim/examples/demo_pddl.py | 11 +- pyrobosim/pyrobosim/core/hallway.py | 1 + pyrobosim/pyrobosim/core/world.py | 11 +- .../pddlstream/domains/01_simple/domain.pddl | 3 + .../pddlstream/domains/02_derived/domain.pddl | 3 + .../domains/03_nav_stream/domain.pddl | 3 + .../domains/04_nav_manip_stream/domain.pddl | 3 + .../domains/05_nav_grasp_stream/domain.pddl | 3 + .../domains/06_open_close_detect/domain.pddl | 251 ++++++++++++++++++ .../domains/06_open_close_detect/streams.pddl | 29 ++ pyrobosim/pyrobosim/planning/actions.py | 2 + .../planning/pddlstream/default_mappings.py | 2 + .../planning/pddlstream/primitives.py | 24 ++ .../pyrobosim/planning/pddlstream/utils.py | 31 ++- pyrobosim_msgs/msg/LocationState.msg | 2 + .../examples/demo_pddl_goal_publisher.py | 21 ++ pyrobosim_ros/examples/demo_pddl_planner.py | 1 + pyrobosim_ros/launch/demo_pddl.launch.py | 2 +- pyrobosim_ros/pyrobosim_ros/ros_interface.py | 11 +- test/planning/test_task_objects.py | 4 +- 21 files changed, 408 insertions(+), 11 deletions(-) create mode 100644 pyrobosim/pyrobosim/data/pddlstream/domains/06_open_close_detect/domain.pddl create mode 100644 pyrobosim/pyrobosim/data/pddlstream/domains/06_open_close_detect/streams.pddl diff --git a/docs/source/usage/tamp.rst b/docs/source/usage/tamp.rst index 6d225e54..bd2d499d 100644 --- a/docs/source/usage/tamp.rst +++ b/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. diff --git a/pyrobosim/examples/demo_pddl.py b/pyrobosim/examples/demo_pddl.py index 7ce5648d..a75753eb 100755 --- a/pyrobosim/examples/demo_pddl.py +++ b/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 diff --git a/pyrobosim/pyrobosim/core/hallway.py b/pyrobosim/pyrobosim/core/hallway.py index 1d119f0c..85adccd3 100644 --- a/pyrobosim/pyrobosim/core/hallway.py +++ b/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 diff --git a/pyrobosim/pyrobosim/core/world.py b/pyrobosim/pyrobosim/core/world.py index 12060844..dc47c9bb 100644 --- a/pyrobosim/pyrobosim/core/world.py +++ b/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) diff --git a/pyrobosim/pyrobosim/data/pddlstream/domains/01_simple/domain.pddl b/pyrobosim/pyrobosim/data/pddlstream/domains/01_simple/domain.pddl index 4840575c..bdbd16d5 100644 --- a/pyrobosim/pyrobosim/data/pddlstream/domains/01_simple/domain.pddl +++ b/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)) diff --git a/pyrobosim/pyrobosim/data/pddlstream/domains/02_derived/domain.pddl b/pyrobosim/pyrobosim/data/pddlstream/domains/02_derived/domain.pddl index 5fe1302b..832c3fcc 100644 --- a/pyrobosim/pyrobosim/data/pddlstream/domains/02_derived/domain.pddl +++ b/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)) diff --git a/pyrobosim/pyrobosim/data/pddlstream/domains/03_nav_stream/domain.pddl b/pyrobosim/pyrobosim/data/pddlstream/domains/03_nav_stream/domain.pddl index f4355cb2..845ed007 100644 --- a/pyrobosim/pyrobosim/data/pddlstream/domains/03_nav_stream/domain.pddl +++ b/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)) diff --git a/pyrobosim/pyrobosim/data/pddlstream/domains/04_nav_manip_stream/domain.pddl b/pyrobosim/pyrobosim/data/pddlstream/domains/04_nav_manip_stream/domain.pddl index 11cfa992..9ee59428 100644 --- a/pyrobosim/pyrobosim/data/pddlstream/domains/04_nav_manip_stream/domain.pddl +++ b/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) diff --git a/pyrobosim/pyrobosim/data/pddlstream/domains/05_nav_grasp_stream/domain.pddl b/pyrobosim/pyrobosim/data/pddlstream/domains/05_nav_grasp_stream/domain.pddl index 5ce08c59..d7c24e73 100644 --- a/pyrobosim/pyrobosim/data/pddlstream/domains/05_nav_grasp_stream/domain.pddl +++ b/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) diff --git a/pyrobosim/pyrobosim/data/pddlstream/domains/06_open_close_detect/domain.pddl b/pyrobosim/pyrobosim/data/pddlstream/domains/06_open_close_detect/domain.pddl new file mode 100644 index 00000000..0cf48b28 --- /dev/null +++ b/pyrobosim/pyrobosim/data/pddlstream/domains/06_open_close_detect/domain.pddl @@ -0,0 +1,251 @@ +; PDDL PLANNING DOMAIN (OPEN, CLOSE, AND DETECT) +; +; This planning domain contains the original `navigate`, `pick`, and `place` actions, +; as well as `open`, `close`, and `detect` actions. +; +; All actions are symbolic, meaning there are no different types of grasps +; or feasibility checks, under the assumption that a downstream planner exists. +; +; Accompanying streams are defined in the `streams.pddl` file. + + +(define (domain domain_open_close_detect) + (:requirements :strips :equality) + (:predicates + ; Static predicates + (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 + + ; Fluent predicates + (HandEmpty ?r) ; Whether the robot's gripper is empty + (CanMove ?r) ; Whether the robot can move (prevents duplicate moves) + (Holding ?r ?o) ; Object the robot is holding + (At ?o ?l) ; Robot/Object's location + (AtRoom ?l ?r) ; Location's corresponding room + (Has ?loc ?entity) ; Check existence of entities (object instances or types) in locations + (HasNone ?loc ?entity) ; Check nonexistence of entities (object instances or types) in locations + (HasAll ?loc ?entity) ; Check exclusivity of entities (object instances or types) in locations + (IsOpen ?l) ; Whether a location is open + (IsLocked ?l) ; Whether a location is locked (robot cannot open it) + (IsObserved ?l) ; Whether a location has been observed with a detect action + ) + + ; FUNCTIONS : See their descriptions in the stream PDDL file + (:functions + (Dist ?l1 ?l2) + (PickPlaceCost) + ) + + ; ACTIONS + ; NAVIGATE: Moves the robot from one location to the other + (:action navigate + :parameters (?r ?l1 ?l2) + :precondition (and (Robot ?r) + (CanMove ?r) + (Location ?l1) + (Location ?l2) + (At ?r ?l1)) + :effect (and (not (CanMove ?r)) + (At ?r ?l2) (not (At ?r ?l1)) + (increase (total-cost) (Dist ?l1 ?l2))) + ) + + ; PICK: Picks up an object from a specified location + (:action pick + :parameters (?r ?o ?l) + :precondition (and (Robot ?r) + (Obj ?o) + (Location ?l) + (not (Room ?l)) + (not (Hallway ?l)) + (HandEmpty ?r) + (At ?r ?l) + (IsObserved ?l) + (IsOpen ?l) + (At ?o ?l)) + :effect (and (Holding ?r ?o) (CanMove ?r) + (not (HandEmpty ?r)) + (not (At ?o ?l)) + (increase (total-cost) (PickPlaceCost ?l ?o))) + ) + + ; PLACE: Places an object in a specified location + (:action place + :parameters (?r ?o ?l) + :precondition (and (Robot ?r) + (Obj ?o) + (Location ?l) + (not (Room ?l)) + (not (Hallway ?l)) + (At ?r ?l) + (IsObserved ?l) + (IsOpen ?l) + (not (HandEmpty ?r)) + (Holding ?r ?o)) + :effect (and (HandEmpty ?r) (CanMove ?r) + (At ?o ?l) + (not (Holding ?r ?o)) + (increase (total-cost) (PickPlaceCost ?l ?o))) + ) + + ; DETECT: Detects objects in a specified location + (:action detect + :parameters (?r ?l) + :precondition (and (Robot ?r) + (Location ?l) + (not (Room ?l)) + (not (Hallway ?l)) + (At ?r ?l) + (not (IsObserved ?l)) + (IsOpen ?l)) + :effect (and (IsObserved ?l) + (increase (total-cost) (DetectCost ?l))) + ) + + ; OPEN: Opens a specified location + (:action open + :parameters (?r ?l) + :precondition (and (Robot ?r) + (Location ?l) + (not (Room ?l)) + (At ?r ?l) + (HandEmpty ?r) + (not (IsOpen ?l)) + (not (IsLocked ?l))) + :effect (and (IsOpen ?l) + (increase (total-cost) (OpenCloseCost ?l))) + ) + + ; CLOSE: Closes a specified location + (:action close + :parameters (?r ?l) + :precondition (and (Robot ?r) + (Location ?l) + (not (Room ?l)) + (At ?r ?l) + (HandEmpty ?r) + (IsOpen ?l) + (not (IsLocked ?l))) + :effect (and (not (IsOpen ?l)) + (increase (total-cost) (OpenCloseCost ?l))) + ) + + + ; DERIVED PREDICATES + ; HAS: Checks locations using entity and location types or instances, + ; or even room names as locations + (:derived (Has ?loc ?entity) + (or + ; CASE 1: Location and entity specified as instances + (and (or (Obj ?entity) (Robot ?entity)) + (Location ?loc) + (or (At ?entity ?loc) + (exists (?s) + (and (Room ?loc) (Location ?s) + (At ?entity ?s) (AtRoom ?s ?loc)) + ) + ) + ) + ; CASE 2: Location is a type, entity is instance + (exists (?l) (and (or (Obj ?entity) (Robot ?entity)) + (Location ?l) + (or (and (Type ?loc) (Is ?l ?loc)) + (and (Room ?loc) (AtRoom ?l ?loc))) + (At ?entity ?l)) + ) + ; CASE 3: Location is instance, entity is a type + (exists (?o) (and (Type ?entity) (Obj ?o) + (Location ?loc) + (Is ?o ?entity) + (or (At ?o ?loc) + (exists (?s) + (and (Room ?loc) (Location ?s) + (At ?o ?s) (AtRoom ?s ?loc))) + ) + ) + ) + ; CASE 4: Location and object specified as types + (exists (?o ?l) (and (Type ?entity) (Obj ?o) + (Type ?loc) (Location ?l) + (or (and (Type ?loc) (Is ?l ?loc)) + (and (Room ?loc) (AtRoom ?l ?loc))) + (Is ?o ?entity) + (At ?o ?l) + ) + ) + ; CASE 5: Robot holding an object instance + (and (Robot ?loc) (Obj ?entity) + (Holding ?loc ?entity) + ) + (exists (?r) + (and (Robot ?r) (Location ?loc) (Obj ?entity) + (Holding ?r ?entity) + (or (At ?r ?loc) + (and (Room ?loc) (exists (?s) + (and (Location ?s) (At ?r ?s) (AtRoom ?s ?loc)))) + ) + ) + ) + ; CASE 6: Robot holding an object type + (exists (?o) + (and (Robot ?loc) (Obj ?o) (Holding ?loc ?o) + (Type ?entity) (Is ?o ?entity)) + ) + (exists (?r ?o) + (and (Robot ?r) (Location ?loc) (Obj ?o) (Holding ?r ?o) + (Type ?entity) (Is ?o ?entity) + (or (At ?r ?loc) + (and (Room ?loc) (exists (?s) + (and (Location ?s) (At ?r ?s) (AtRoom ?s ?loc)))) + ) + ) + ) + ) + ) + + ; HASNONE: The opposite of "HAS". + ; Checks that a location or location type has no object type + ; or instances of an object type + (:derived (HasNone ?loc ?entity) + (not (Has ?loc ?entity)) + ) + + ; HASALL: A variant of "HAS" for all rather than any objects. + ; Checks that an object type or all instances of an object type are in a + ; specific location or location type + (:derived (HasAll ?loc ?objtype) + (or + ; CASE 1: Location is an instance + (forall (?o) + (imply + (and (Obj ?o) (Type ?objtype) (Is ?o ?objtype)) + (and (Location ?loc) + (or (At ?o ?loc) + (exists (?s) + (and (Room ?loc) (Location ?s) + (At ?o ?s) (At ?s ?loc))) + ) + ) + ) + ) + ; CASE 2: Location is a type + (forall (?o) + (imply + (and (Obj ?o) (Type ?objtype) (Is ?o ?objtype)) + (exists (?l) (and + (Location ?l) + (or (and (Type ?loc) (Is ?l ?loc)) + (and (Room ?loc) (At ?l ?loc))) + (At ?o ?l)) + ) + ) + ) + ) + ) + +) diff --git a/pyrobosim/pyrobosim/data/pddlstream/domains/06_open_close_detect/streams.pddl b/pyrobosim/pyrobosim/data/pddlstream/domains/06_open_close_detect/streams.pddl new file mode 100644 index 00000000..39dd53be --- /dev/null +++ b/pyrobosim/pyrobosim/data/pddlstream/domains/06_open_close_detect/streams.pddl @@ -0,0 +1,29 @@ +; STREAMS FOR PDDL PLANNING DOMAIN (OPEN, CLOSE, AND DETECT) +; +; Contains simple cost functions for actions. +; +; Accompanying planning domain defined in the `domain.pddl` file. + +(define (stream stream_open_close_detect) + + ; DIST: Distance between two locations. + (:function (Dist ?l1 ?l2) + (and (Location ?l1) (Location ?l2)) + ) + + ; PICKPLACECOST: Cost to perform pick and place at a location. + (:function (PickPlaceCost ?l ?o) + (and (Location ?l) (Obj ?o)) + ) + + ; DETECTCOST: Cost to detect objects at a location. + (:function (DetectCost ?l) + (and (Location ?l)) + ) + + ; OPENCLOSECOST: Cost to open or close a location. + (:function (OpenCloseCost ?l) + (and (Location ?l)) + ) + +) diff --git a/pyrobosim/pyrobosim/planning/actions.py b/pyrobosim/pyrobosim/planning/actions.py index 14bf2d99..f73ef3cb 100644 --- a/pyrobosim/pyrobosim/planning/actions.py +++ b/pyrobosim/pyrobosim/planning/actions.py @@ -202,6 +202,8 @@ def __repr__(self): act_str += f" {self.object}" else: act_str += " objects" + if self.target_location is not None: + act_str += f" at {self.target_location}" # OPEN / CLOSE elif self.type == "open": act_str += "Open" diff --git a/pyrobosim/pyrobosim/planning/pddlstream/default_mappings.py b/pyrobosim/pyrobosim/planning/pddlstream/default_mappings.py index 6a6101f7..eb5aa986 100644 --- a/pyrobosim/pyrobosim/planning/pddlstream/default_mappings.py +++ b/pyrobosim/pyrobosim/planning/pddlstream/default_mappings.py @@ -29,6 +29,8 @@ def get_stream_map(world, robot): "PickPlaceCost": primitives.get_pick_place_cost, "PickPlaceAtPoseCost": primitives.get_pick_place_at_pose_cost, "GraspAtPoseCost": primitives.get_grasp_at_pose_cost, + "DetectCost": primitives.get_detect_cost, + "OpenCloseCost": primitives.get_open_close_cost, "PathLength": primitives.get_path_length, # Streams (that sample) "s-navpose": from_list_fn(primitives.get_nav_poses), diff --git a/pyrobosim/pyrobosim/planning/pddlstream/primitives.py b/pyrobosim/pyrobosim/planning/pddlstream/primitives.py index 9777a670..06e365b9 100644 --- a/pyrobosim/pyrobosim/planning/pddlstream/primitives.py +++ b/pyrobosim/pyrobosim/planning/pddlstream/primitives.py @@ -70,6 +70,30 @@ def get_grasp_at_pose_cost(g, pr): return distance_cost + face_cost +def get_detect_cost(loc): + """ + Estimates the cost of detecting objects at a location. + + :param loc: Location where the detect action occurs. + :type loc: Location + :return: Cost of performing action. + :rtype: float + """ + return 0.5 + + +def get_open_close_cost(loc): + """ + Estimates the detection cost of opening or closing a location. + + :param loc: Location where the open or close action occurs. + :type loc: Location + :return: Cost of performing action. + :rtype: float + """ + return 1.0 + + def get_straight_line_distance(l1, l2): """ Optimistically estimate the distance between two locations by getting the diff --git a/pyrobosim/pyrobosim/planning/pddlstream/utils.py b/pyrobosim/pyrobosim/planning/pddlstream/utils.py index 0178413c..f98f267d 100644 --- a/pyrobosim/pyrobosim/planning/pddlstream/utils.py +++ b/pyrobosim/pyrobosim/planning/pddlstream/utils.py @@ -74,7 +74,7 @@ def world_to_pddlstream_init(world, robot): ] # Loop through all the locations and their relationships. - # This includes rooms and object spawns (which are children of locations). + # This includes rooms, hallways, and object spawns (which are children of locations). for room in world.rooms: init.append(("Room", room)) init.append(("Location", room)) @@ -84,13 +84,29 @@ def world_to_pddlstream_init(world, robot): init.append(("Location", spawn)) init.append(("Is", spawn, loc.category)) init.append(("AtRoom", spawn, loc.parent)) + # TODO: Note that open/locked status pertains to the entire location, + # but the PDDL domain only knows about the individual object spawns. + # As such, this may not work on locations that have multiple object spawns. + if loc.is_open: + init.append(("IsOpen", spawn)) + if loc.is_locked: + init.append(("IsLocked", spawn)) + # TODO: This assumes no locations have been observed at the start of planning. + # You could add something like: init.append(("IsObserved", spawn)) loc_categories.add(loc.category) for loc_cat in loc_categories: init.append(("Type", loc_cat)) + for hallway in world.hallways: + init.append(("Hallway", hallway)) + init.append(("Location", hallway)) + if hallway.is_open: + init.append(("IsOpen", hallway)) + if hallway.is_locked: + init.append(("IsLocked", hallway)) # Loop through all the objects and their relationships. obj_categories = set() - for obj in world.objects: + for obj in robot.get_known_objects(): init.append(("Obj", obj)) init.append(("Is", obj, obj.category)) # If the object is the current manipulated object, change the state of @@ -166,7 +182,6 @@ def pddlstream_solution_to_plan(solution, robot): # Convert the PDDL action to a TaskAction act = TaskAction(act_pddl.name) act.robot = robot - # Parse a NAVIGATE action if act.type == "navigate": act.source_location = act_pddl.args[1] act.target_location = act_pddl.args[2] @@ -174,8 +189,8 @@ def pddlstream_solution_to_plan(solution, robot): for arg in act_pddl.args[3:]: if isinstance(arg, Path): act.path = arg - # Parse a PICK or PLACE action - elif act.type == "pick" or act.type == "place": + + elif act.type in ("pick", "place"): act.object = act_pddl.args[1] act.target_location = act_pddl.args[2] # If a pick/place pose is specified, add it. @@ -189,6 +204,12 @@ def pddlstream_solution_to_plan(solution, robot): if isinstance(arg, Grasp): act.pose = arg.origin_wrt_world + elif act.type in ("detect", "open", "close"): + act.target_location = act_pddl.args[1] + + else: + raise ValueError(f"No known action type: {act.type}") + # Add the action to the task plan plan_out.actions.append(act) diff --git a/pyrobosim_msgs/msg/LocationState.msg b/pyrobosim_msgs/msg/LocationState.msg index 69e33455..deaaf3fd 100644 --- a/pyrobosim_msgs/msg/LocationState.msg +++ b/pyrobosim_msgs/msg/LocationState.msg @@ -7,3 +7,5 @@ string category # Dynamic data string parent geometry_msgs/Pose pose +bool is_open +bool is_locked diff --git a/pyrobosim_ros/examples/demo_pddl_goal_publisher.py b/pyrobosim_ros/examples/demo_pddl_goal_publisher.py index 00324dec..64d05c0d 100755 --- a/pyrobosim_ros/examples/demo_pddl_goal_publisher.py +++ b/pyrobosim_ros/examples/demo_pddl_goal_publisher.py @@ -9,6 +9,7 @@ import time from pyrobosim_msgs.msg import GoalPredicate, GoalSpecification +from pyrobosim_msgs.srv import SetLocationState class GoalPublisher(Node): @@ -19,6 +20,11 @@ def __init__(self): self.declare_parameter("example", value="01_simple") self.declare_parameter("verbose", value=True) + # Create a service to set the location state (for the open/close/detect example) + self.set_location_state_client = self.create_client( + SetLocationState, "set_location_state" + ) + # Publisher for a goal specification self.goalspec_pub = self.create_publisher( GoalSpecification, "goal_specification", 10 @@ -47,6 +53,7 @@ def __init__(self): "03_nav_stream", "04_nav_manip_stream", "05_nav_grasp_stream", + "06_open_close_detect", ]: # Goal specification for derived predicate example. goal_predicates = [ @@ -55,6 +62,20 @@ def __init__(self): GoalPredicate(type="HasNone", args=("bathroom", "banana")), GoalPredicate(type="HasAll", args=("table", "water")), ] + # If running the open/close/detect example, close the desk location. + if example == "06_open_close_detect": + future = self.set_location_state_client.call_async( + SetLocationState.Request( + location_name="desk0", + open=False, + lock=False, + ) + ) + start_time = time.time() + while not future.done(): + rclpy.spin_once(self, timeout_sec=0.1) + if time.time() - start_time > 2.0: + raise TimeoutError("Failed to close location before planning.") else: self.get_logger().info(f"Invalid example: {example}") return diff --git a/pyrobosim_ros/examples/demo_pddl_planner.py b/pyrobosim_ros/examples/demo_pddl_planner.py index 7d00b0fc..6557c491 100755 --- a/pyrobosim_ros/examples/demo_pddl_planner.py +++ b/pyrobosim_ros/examples/demo_pddl_planner.py @@ -88,6 +88,7 @@ def __init__(self): "03_nav_stream", "04_nav_manip_stream", "05_nav_grasp_stream", + "06_open_close_detect", ]: # Task specification for derived predicate example. self.latest_goal = [ diff --git a/pyrobosim_ros/launch/demo_pddl.launch.py b/pyrobosim_ros/launch/demo_pddl.launch.py index af9c2e2b..3ea5bcfd 100644 --- a/pyrobosim_ros/launch/demo_pddl.launch.py +++ b/pyrobosim_ros/launch/demo_pddl.launch.py @@ -32,7 +32,7 @@ def generate_launch_description(): "example", default_value=TextSubstitution(text="01_simple"), description="Example name, must be one of " - + "(01_simple, 02_derived, 03_nav_stream, 04_nav_manip_stream, 05_nav_grasp_stream)", + + "(01_simple, 02_derived, 03_nav_stream, 04_nav_manip_stream, 05_nav_grasp_stream, 06_open_close_detect)", ) verbose_arg = DeclareLaunchArgument( "verbose", diff --git a/pyrobosim_ros/pyrobosim_ros/ros_interface.py b/pyrobosim_ros/pyrobosim_ros/ros_interface.py index e934eb45..2febf749 100644 --- a/pyrobosim_ros/pyrobosim_ros/ros_interface.py +++ b/pyrobosim_ros/pyrobosim_ros/ros_interface.py @@ -461,12 +461,15 @@ def world_state_callback(self, request, response): objects = self.world.objects # Add the object and location states. + # TODO: Support hallway states as well. for loc in self.world.locations: loc_msg = LocationState( name=loc.name, category=loc.category, parent=loc.get_room_name(), pose=pose_to_ros(loc.pose), + is_open=loc.is_open, + is_locked=loc.is_locked, ) response.state.locations.append(loc_msg) for obj in objects: @@ -592,5 +595,11 @@ def update_world_from_state_msg(world, msg): # Update the location states for loc_state in msg.locations: world.update_location( - loc_state.name, room=loc_state.parent, pose=pose_from_ros(loc_state.pose) + loc_state.name, + room=loc_state.parent, + pose=pose_from_ros(loc_state.pose), + is_open=loc_state.is_open, + is_locked=loc_state.is_locked, ) + + # TODO: Update the hallway states once this is supported. diff --git a/test/planning/test_task_objects.py b/test/planning/test_task_objects.py index 73c4d2a1..e5569ba2 100644 --- a/test/planning/test_task_objects.py +++ b/test/planning/test_task_objects.py @@ -128,9 +128,9 @@ def test_print_task_action(capsys): out, _ = capsys.readouterr() assert out == "Detect objects\n" - print(TaskAction("detect", object="banana")) + print(TaskAction("detect", object="banana", target_location="table")) out, _ = capsys.readouterr() - assert out == "Detect banana\n" + assert out == "Detect banana at table\n" print(TaskAction("open")) out, _ = capsys.readouterr()