From 0685cde66592eb768260eb0a9e25121d6262be61 Mon Sep 17 00:00:00 2001 From: Jorge Mendez Date: Tue, 4 Oct 2022 12:24:33 -0400 Subject: [PATCH] Add slice operator and support for ObjectMultiplexer --- predicators/behavior_utils/behavior_utils.py | 50 ++++- .../behavior_utils/motion_planner_fns.py | 25 ++- .../behavior_utils/option_model_fns.py | 27 ++- predicators/envs/behavior.py | 196 ++++++++++++++---- predicators/ground_truth_nsrts.py | 48 ++++- 5 files changed, 287 insertions(+), 59 deletions(-) diff --git a/predicators/behavior_utils/behavior_utils.py b/predicators/behavior_utils/behavior_utils.py index 3dc4b5c517..5a39b58477 100644 --- a/predicators/behavior_utils/behavior_utils.py +++ b/predicators/behavior_utils/behavior_utils.py @@ -37,7 +37,11 @@ 'document', 'bottom_cabinet_no_top', 'folder', 'bottom_cabinet', 'top_cabinet', 'sofa', 'oatmeal', 'chip', 'vegetable_oil', 'sugar', 'cabinet', 'floor', 'pasta', 'sauce', 'electric_refrigerator', 'olive_oil', - 'sugar_jar', 'spaghetti_sauce', 'mayonnaise', 'fridge' + 'sugar_jar', 'spaghetti_sauce', 'mayonnaise', 'fridge', + 'tomato', 'strawberry', 'peach', 'jar', 'carving_knife', 'mushroom', + 'chestnut', 'vidalia_onion', 'table_knife', 'sink', 'teapot', 'tea_bag', + 'lemon', 'stove', 'casserole', 'pocketknife', 'half_tomato', + 'half_mushroom', 'half_chestnut', 'half_vidalia_onion' } PICK_PLACE_OBJECT_TYPES = { 'mineral_water', 'oatmeal', 'blueberry', 'headset', 'jug', 'flank', @@ -122,7 +126,7 @@ 'peppermint', 'cruciferous_vegetable', 'soup_ladle', 'jean', 'teddy', 'chestnut', 'sauce', 'piece_of_cloth', 'whitefish', 'siren', 'balloon', 'celery', 'hot_pepper', 'raisin', 'sugar_jar', 'toy', 'sticky_note', - 't-shirt' + 't-shirt', 'table_knife' } PLACE_ONTOP_SURFACE_OBJECT_TYPES = { 'towel', 'tabletop', 'face', 'brim', 'cheddar', 'chaise_longue', 'stove', @@ -184,6 +188,48 @@ 'bottom_cabinet', 'trash_can' } +SLICEABLE_OBJECT_TYPES = { + 'cucumber', 'cheddar', 'kabob', 'lime', 'gauze', 'cotton', 'bulbous_plant', + 'french_bread', 'mango', 'cauliflower', 'gumbo', 'salami', 'fish', 'blueberry', + 'flank', 'hot_pepper', 'sweet_pepper', 'sauerkraut', 'chickpea', 'trout', 'kale', + 'bun', 'vascular_plant', 'licorice', 'walnut', 'muskmelon', 'shrub', 'drupelet', + 'shank', 'frozen_dessert', 'cherry', 'bell_pepper', 'pork', 'sandwich', 'pumpkin', + 'turkey', 'pepper', 'winter_melon', 'rib', 'pretzel', 'curd', 'summer_squash', + 'melon', 'lettuce', 'honeydew', 'nectarine', 'bean_curd', 'dish', 'gouda', + 'cayenne', 'pear', 'gingerbread', 'apricot', 'noodle', 'sausage', 'plum', 'salmon', + 'cheese', 'mint', 'eggplant', 'marjoram', 'gorgonzola', 'beefsteak', 'cabbage', + 'hazelnut', 'saltwater_fish', 'cupcake', 'solanaceous_vegetable', 'turnip', + 'venison', 'breast', 'bird', 'artichoke', 'pomegranate', 'brownie', 'pea', + 'white_bread', 'soy_sauce', 'taco', 'egg', 'brie', 'ginger', 'rosemary', 'poultry', + 'butter', 'citrus', 'anchovy', 'burrito', 'cornbread', 'grape', 'foil', 'lemon', + 'chicory_escarole', 'marshmallow', 'ham', 'sushi', 'prosciutto', 'chicken', + 'biscuit', 'rayon', 'parsley', 'red_salmon', 'fillet', 'chives', 'pepperoni', + 'muffin', 'thyme', 'baguet', 'potato', 'radish', 'cracker', 'clove', + 'alliaceous_plant', 'beef', 'peanut', 'tinfoil', 'julienne', 'garlic', 'scone', + 'crust', 'cantaloup', 'porterhouse', 'avocado', 'pasta', 'pie', 'lint', 'pineapple', + 'gooseberry', 'cut_of_pork', 'cheesecake', 'tomato', 'dried_fruit', 'banana', + 'shallot', 'bow', 'chocolate', 'lasagna', 'blackberry', 'piece', 'salad_green', + 'meat_loaf', 'pizza', 'brisket', 'bagel', 'freshwater_fish', 'dill', 'ice_lolly', + 'asparagus', 'toast', 'watermelon', 'cut_of_beef', 'quick_bread', 'mushroom', + 'olive', 'date', 'sheet_metal', 'saute', 'bacon', 'raspberry', 'alfalfa', 'tart', + 'dough', 'pancake', 'tortilla', 'fig', 'strawberry', 'chili', 'knitwear', 'coconut', + 'onion', 'spinach', 'carrot', 'grapefruit', 'mozzarella', 'pomelo', 'peach', + 'drumstick', 'prawn', 'silver_salmon', 'parmesan', 'feijoa', 'pome', 'papaya', + 'basil', 'chicory', 'almond', 'green_onion', 'woody_plant', 'head_cabbage', 'meatball', + 'comfrey', 'drupe', 'currant', 'crouton', 'bleu', 'cos', 'margarine', 'radicchio', + 'pita', 'nut', 'tuna', 'sirloin', 'veal', 'flatbread', 'steak', 'broccoli', 'casserole', + 'peppermint', 'cruciferous_vegetable', 'cornice', 'loaf_of_bread', 'coriander', 'lamb', + 'chestnut', 'liliaceous_plant', 'rump', 'zucchini', 'whitefish', 'vidalia_onion', + 'kiwi', 'mandarin', 'game', 'celery', 'squash', 'hamburger', 'acetate_rayon', 'orange', + 'cutlet', 'apple', 'tenderloin', 'fudge', 'raisin' +} + +SLICER_OBJECT_TYPES = { + 'hacksaw', 'stiletto', 'carving_knife', 'clipper', 'garden_tool', 'dagger', + 'table_knife', 'razor', 'shaver', 'pruning_saw', 'knife', 'chisel', 'lawn_mower', + 'saw', 'edge_tool', 'cleaver', 'scissors', 'handsaw', 'pocketknife', 'trimmer', + 'shear', 'pruner', 'cutter' +} def get_aabb_volume(lo: Array, hi: Array) -> float: """Simple utility function to compute the volume of an aabb. diff --git a/predicators/behavior_utils/motion_planner_fns.py b/predicators/behavior_utils/motion_planner_fns.py index 041c0e1965..453a5c5620 100644 --- a/predicators/behavior_utils/motion_planner_fns.py +++ b/predicators/behavior_utils/motion_planner_fns.py @@ -22,6 +22,7 @@ from igibson.object_states.on_floor import \ RoomFloor # pylint: disable=unused-import from igibson.objects.articulated_object import URDFObject + from igibson.objects.multi_object_wrappers import ObjectMultiplexer from igibson.utils.behavior_robot_planning_utils import \ plan_base_motion_br, plan_hand_motion_br @@ -86,7 +87,7 @@ def sample_fn(env: "BehaviorEnv", if not isinstance( obj, - URDFObject): # must be a URDFObject so we can get its position! + (URDFObject, ObjectMultiplexer)): # must be a URDFObject so we can get its position! logging.error("ERROR! Object to navigate to is not valid (not an " "instance of URDFObject).") p.restoreState(state) @@ -201,7 +202,7 @@ def make_grasp_plan( # If the object we're trying to grasp doesn't have all the attributes # we'll need for assistive grasping, fail and return None - if not (isinstance(obj, URDFObject) and hasattr(obj, "states") + if not (isinstance(obj, (URDFObject, ObjectMultiplexer)) and hasattr(obj, "states") and object_states.AABB in obj.states): logging.info(f"PRIMITIVE: grasp {obj.name} fail, no object") return None @@ -385,7 +386,7 @@ def make_place_plan( obj_in_hand_idx = env.robots[0].parts["right_hand"].object_in_hand obj_in_hand = [ obj for obj in env.scene.get_objects() - if obj.get_body_id() == obj_in_hand_idx + if (not isinstance(obj, ObjectMultiplexer) or obj.current_index == 0) and obj.get_body_id() == obj_in_hand_idx ][0] logging.info(f"PRIMITIVE: attempt to place {obj_in_hand.name} ontop" f"/inside {obj.name} with params {place_rel_pos}") @@ -402,7 +403,7 @@ def make_place_plan( return None # if the object is not a urdf object, fail and return None - if not isinstance(obj, URDFObject): + if not isinstance(obj, (URDFObject, ObjectMultiplexer)): logging.info(f"PRIMITIVE: place {obj_in_hand.name} ontop/inside " f"{obj.name} fail, too far") return None @@ -423,7 +424,7 @@ def make_place_plan( obj_in_hand_idx = env.robots[0].parts["right_hand"].object_in_hand obj_in_hand = [ obj for obj in env.scene.get_objects() - if obj.get_body_id() == obj_in_hand_idx + if (not isinstance(obj, ObjectMultiplexer) or obj.current_index == 0) and obj.get_body_id() == obj_in_hand_idx ][0] x, y, z = np.add(place_rel_pos, obj.get_position()) hand_x, hand_y, hand_z = env.robots[0].parts["right_hand"].get_position() @@ -436,7 +437,19 @@ def make_place_plan( maxz = max(z, hand_z) + 0.5 obstacles = get_scene_body_ids(env, include_self=False) - obstacles.remove(env.robots[0].parts["right_hand"].object_in_hand) + if isinstance(obj_in_hand, ObjectMultiplexer): + # assert obj_in_hand.current_index == 0 + # obj_in_hand.set_selection(1) + # obj_in_hand_idx_0 = obj_in_hand.objects[0].get_body_id() + # obj_in_hand_idx_1 = obj_in_hand.objects[1].get_body_id() + # obstacles.remove(obj_in_hand_idx_0) + # obstacles.remove(obj_in_hand_idx_1) + # obj_in_hand.set_selection(0) + # Note: it seems that object multiplexers don't show up in the list + # of obstacles, so we can just skip this step + pass + else: + obstacles.remove(env.robots[0].parts["right_hand"].object_in_hand) end_conf = [ x, y, diff --git a/predicators/behavior_utils/option_model_fns.py b/predicators/behavior_utils/option_model_fns.py index f73b08761e..bbb259d243 100644 --- a/predicators/behavior_utils/option_model_fns.py +++ b/predicators/behavior_utils/option_model_fns.py @@ -15,6 +15,7 @@ BehaviorEnv # pylint: disable=unused-import from igibson.objects.articulated_object import \ URDFObject # pylint: disable=unused-import + from igibson.objects.multi_object_wrappers import ObjectMultiplexer except (ImportError, ModuleNotFoundError) as e: pass @@ -210,7 +211,7 @@ def placeInsideObjectOptionModel(_init_state: State, obj_in_hand_idx = env.robots[0].parts["right_hand"].object_in_hand obj_in_hand = [ obj for obj in env.scene.get_objects() - if obj.get_body_id() == obj_in_hand_idx + if (not isinstance(obj, ObjectMultiplexer) or obj.current_index == 0) and obj.get_body_id() == obj_in_hand_idx ][0] rh_orig_grasp_position = env.robots[0].parts[ "right_hand"].get_position() @@ -268,3 +269,27 @@ def placeInsideObjectOptionModel(_init_state: State, env.step(np.zeros(env.action_space.shape)) return placeInsideObjectOptionModel + +def create_slice_option_model( + plan: List[List[float]], _original_orientation: List[List[float]], + obj_to_slice: "URDFObject") -> Callable[[State, "BehaviorEnv"], None]: + """Instantiates and returns a slice option model given a dummy plan.""" + del plan + + def sliceObjectOptionModel(_init_state: State, env: "BehaviorEnv") -> None: + logging.info(f"PRIMITIVE: Attempting to slice {obj_to_slice.name}") + if np.linalg.norm( + np.array(obj_to_slice.get_position()) - + np.array(env.robots[0].get_position())) < 2: + if hasattr(obj_to_slice, + "states") and object_states.Sliced in obj_to_slice.states: + obj_to_slice.states[object_states.Sliced].set_value(True) + else: + logging.info("PRIMITIVE slice failed, cannot be sliced") + else: + logging.info("PRIMITIVE slice failed, too far") + obj_to_slice.force_wakeup() + # Step the simulator to update visuals. + env.step(np.zeros(env.action_space.shape)) + + return sliceObjectOptionModel diff --git a/predicators/envs/behavior.py b/predicators/envs/behavior.py index 804caabe99..9d66abff5b 100644 --- a/predicators/envs/behavior.py +++ b/predicators/envs/behavior.py @@ -11,30 +11,31 @@ import numpy as np from numpy.random._generator import Generator -try: - import bddl - import igibson - import pybullet as pyb - from igibson import object_states - from igibson.activity.bddl_backend import SUPPORTED_PREDICATES, \ - ObjectStateBinaryPredicate, ObjectStateUnaryPredicate - from igibson.envs import behavior_env - from igibson.object_states.on_floor import RoomFloor - from igibson.objects.articulated_object import \ - ArticulatedObject # pylint: disable=unused-import - from igibson.objects.articulated_object import \ - URDFObject # pylint: disable=unused-import - from igibson.robots.behavior_robot import BRBody - from igibson.simulator import Simulator # pylint: disable=unused-import - from igibson.utils.checkpoint_utils import save_checkpoint - from igibson.utils.utils import modify_config_file - - _BEHAVIOR_IMPORTED = True - bddl.set_backend("iGibson") # pylint: disable=no-member - if not os.path.exists("tmp_behavior_states/"): - os.makedirs("tmp_behavior_states/") -except (ImportError, ModuleNotFoundError) as e: - _BEHAVIOR_IMPORTED = False +# try: +import bddl +import igibson +import pybullet as pyb +from igibson import object_states +from igibson.activity.bddl_backend import SUPPORTED_PREDICATES, \ + ObjectStateBinaryPredicate, ObjectStateUnaryPredicate +from igibson.envs import behavior_env +from igibson.object_states.on_floor import RoomFloor +from igibson.objects.articulated_object import \ + ArticulatedObject # pylint: disable=unused-import +from igibson.objects.articulated_object import \ + URDFObject # pylint: disable=unused-import +from igibson.objects.multi_object_wrappers import ObjectMultiplexer +from igibson.robots.behavior_robot import BRBody +from igibson.simulator import Simulator # pylint: disable=unused-import +from igibson.utils.checkpoint_utils import save_checkpoint +from igibson.utils.utils import modify_config_file + +_BEHAVIOR_IMPORTED = True +bddl.set_backend("iGibson") # pylint: disable=no-member +if not os.path.exists("tmp_behavior_states/"): + os.makedirs("tmp_behavior_states/") +# except (ImportError, ModuleNotFoundError) as e: +# _BEHAVIOR_IMPORTED = False from gym.spaces import Box from predicators import utils @@ -47,7 +48,8 @@ from predicators.behavior_utils.option_model_fns import \ create_close_option_model, create_grasp_option_model, \ create_navigate_option_model, create_open_option_model, \ - create_place_inside_option_model, create_place_option_model + create_place_inside_option_model, create_place_option_model, \ + create_slice_option_model from predicators.envs import BaseEnv from predicators.settings import CFG from predicators.structs import Action, Array, GroundAtom, Object, \ @@ -143,7 +145,8 @@ def set_options(self) -> None: Callable[[State, "behavior_env.BehaviorEnv"], None]]] = [ create_navigate_option_model, create_grasp_option_model, create_place_option_model, create_open_option_model, - create_close_option_model, create_place_inside_option_model + create_close_option_model, create_place_inside_option_model, + create_slice_option_model, ] # name, planner_fn, option_policy_fn, option_model_fn, @@ -161,6 +164,8 @@ def set_options(self) -> None: option_model_fns[4], 3, 1, (-1.0, 1.0)), ("PlaceInside", planner_fns[2], option_policy_fns[3], option_model_fns[5], 3, 1, (-1.0, 1.0)), + ("Slice", planner_fns[3], option_policy_fns[3], + option_model_fns[6], 3, 1, (-1.0, 1.0)) ] self._options: Set[ParameterizedOption] = set() for (name, planner_fn, policy_fn, option_model_fn, param_dim, num_args, @@ -346,7 +351,7 @@ def _get_task_goal(self) -> Set[GroundAtom]: self._name_to_ig_object(t) for t in head_expr.terms[obj_start_idx:] ] - objects = [self._ig_object_to_object(i) for i in ig_objs] + objects = [obj for i in ig_objs for obj in self._ig_object_to_object(i) ] pred_name = self._create_type_combo_name(bddl_name, [o.type for o in objects]) pred = self._name_to_predicate(pred_name) @@ -408,6 +413,10 @@ def predicates(self) -> Set[Predicate]: ("openable", self._openable_classifier, 1), ("not-openable", self._not_openable_classifier, 1), ("closed", self._closed_classifier, 1), + ("sliceable", self._sliceable_classifier, 1), + ("slicer", self._slicer_classifier, 1), + ("not-sliced", self._not_sliced_classifier, 1), + ("sliced", self._sliced_classifier, 1) ] for name, classifier, arity in custom_predicate_specs: @@ -416,7 +425,6 @@ def predicates(self) -> Set[Predicate]: pred_name = self._create_type_combo_name(name, type_combo) pred = Predicate(pred_name, list(type_combo), classifier) predicates.add(pred) - return predicates @property @@ -503,7 +511,22 @@ def render_state(self, "behavior_mode in settings.py instead") def _get_task_relevant_objects(self) -> List["ArticulatedObject"]: - return list(self.igibson_behavior_env.task.object_scope.values()) + initial_list = list(self.igibson_behavior_env.task.object_scope.values()) + + # NOTE: sliceable objects are multiplexers with one URDFObject (which + # is used as the object while not_sliced) and one GroupedObject (which + # contains two halves of the object to be treated as individual objects) + # after slicing. Therefore, we add the multiplexer as a single object + # (handled above in initial_list) and each of the two elements in the + # grouped object (handled below) + multiplexed_list = [] + # for obj in initial_list: + # if isinstance(obj, ObjectMultiplexer): + # obj.multiplexer.set_selection(1) + # for sub_obj in obj.objects: + # multiplexed_list.append(sub_obj) + # obj.multiplexer.set_selection(0) + return initial_list + multiplexed_list def set_igibson_behavior_env(self, task_num: int, task_instance_id: int, seed: int) -> None: @@ -529,8 +552,9 @@ def set_igibson_behavior_env(self, task_num: int, task_instance_id: int, self.igibson_behavior_env.step( np.zeros(self.igibson_behavior_env.action_space.shape)) ig_objs_bddl_scope = [ - self._ig_object_name(obj) - for obj in self._get_task_relevant_objects() + self._ig_object_name(obj)[0] + # for obj in self._get_task_relevant_objects() + for obj in self.igibson_behavior_env.task.object_scope.values() ] if None not in ig_objs_bddl_scope or env_creation_attempts > 9: break @@ -545,7 +569,7 @@ def set_igibson_behavior_env(self, task_num: int, task_instance_id: int, # Do not add @functools.lru_cache(maxsize=None) here this will # lead to wrong mappings when we load a different scene - def _ig_object_to_object(self, ig_obj: "ArticulatedObject") -> Object: + def _ig_object_to_object(self, ig_obj: "ArticulatedObject") -> List[Object]: type_name = ig_obj.category # NOTE: Since we don't necessarily have the full set of # types we might need to solve a new domain, it is often @@ -558,8 +582,11 @@ def _ig_object_to_object(self, ig_obj: "ArticulatedObject") -> Object: # if ig_obj.category not in ALL_RELEVANT_OBJECT_TYPES: # print(ig_obj.category) # import ipdb; ipdb.set_trace() - ig_obj_name = self._ig_object_name(ig_obj) - return Object(ig_obj_name, obj_type) + ig_obj_name_list = self._ig_object_name(ig_obj) + obj_list = [] + for ig_obj_name in ig_obj_name_list: + obj_list.append(Object(ig_obj_name, obj_type)) + return obj_list # Do not add @functools.lru_cache(maxsize=None) here this will # lead to wrong mappings when we load a different scene @@ -572,7 +599,13 @@ def object_to_ig_object(self, obj: Object) -> "ArticulatedObject": def _name_to_ig_object(self, name: str) -> "ArticulatedObject": for ig_obj in self._get_task_relevant_objects(): # Name is extended with sub-type in some behavior tasks - if self._ig_object_name(ig_obj).startswith(name): + ig_names = self._ig_object_name(ig_obj) + if len(ig_names) > 1: + assert isinstance(ig_obj, ObjectMultiplexer) and ig_obj.current_index == 1 + for ig_name, sub_ig_obj in zip(ig_names, ig_obj.objects): + if ig_name.startswith(name): + return sub_ig_obj + elif ig_names[0].startswith(name): return ig_obj raise ValueError(f"No IG object found for name {name}.") @@ -588,14 +621,24 @@ def current_ig_state_to_state(self, save_state: bool = True) -> State: iGibson simulator state.""" state_data = {} for ig_obj in self._get_task_relevant_objects(): - obj = self._ig_object_to_object(ig_obj) - # In the future, we may need other object attributes, - # but for the moment, we just need position and orientation. - obj_state = np.hstack([ - ig_obj.get_position(), - ig_obj.get_orientation(), - ]) - state_data[obj] = obj_state + obj_list = self._ig_object_to_object(ig_obj) + if len(obj_list) > 1: + assert isinstance(ig_obj, ObjectMultiplexer) and ig_obj.current_index == 1 + for obj, sub_ig_obj in zip(obj_list, ig_obj.objects): + # In the future, we may need other object attributes, + # but for the moment, we just need position and orientation. + obj_state = np.hstack([ + sub_ig_obj.get_position(), + sub_ig_obj.get_orientation(), + ]) + state_data[obj] = obj_state + else: + obj = obj_list[0] + obj_state = np.hstack([ + ig_obj.get_position(), + ig_obj.get_orientation(), + ]) + state_data[obj] = obj_state # NOTE: we set simulator state to none as a 'dummy' value. # we should never load a simulator state that was saved when @@ -748,14 +791,75 @@ def _closed_classifier(self, state: State, objs: Sequence[Object]) -> bool: return not ig_obj.states[object_states.Open].get_value() return False + def _sliceable_classifier(self, state: State, + objs: Sequence[Object]) -> bool: + if not state.allclose( + self.current_ig_state_to_state(save_state=False)): + load_checkpoint_state(state, self) + assert len(objs) == 1 + ig_obj = self.object_to_ig_object(objs[0]) + obj_sliceable = hasattr( + ig_obj, "states") and object_states.Sliced in ig_obj.states + return obj_sliceable + + def _slicer_classifier(self, state: State, + objs: Sequence[Object]) -> bool: + if not state.allclose( + self.current_ig_state_to_state(save_state=False)): + load_checkpoint_state(state, self) + assert len(objs) == 1 + ig_obj = self.object_to_ig_object(objs[0]) + obj_slicer = hasattr( + ig_obj, "states") and object_states.Slicer in ig_obj.states + return obj_slicer + + def _not_sliced_classifier(self, state: State, objs: Sequence[Object]) -> bool: + if not state.allclose( + self.current_ig_state_to_state(save_state=False)): + load_checkpoint_state(state, self) + assert len(objs) == 1 + ig_obj = self.object_to_ig_object(objs[0]) + # NOTE: If an object is not sliceable, we default to setting + # it be not_sliced, since that's a precondition for grasping + obj_sliceable = self._sliceable_classifier(state, objs) + if obj_sliceable: + return not ig_obj.states[object_states.Sliced].get_value() + return True + + def _sliced_classifier(self, state: State, objs: Sequence[Object]) -> bool: + if not state.allclose( + self.current_ig_state_to_state(save_state=False)): + load_checkpoint_state(state, self) + assert len(objs) == 1 + ig_obj = self.object_to_ig_object(objs[0]) + # NOTE: If an object is not sliceable, we default to setting + # it to not be not_sliced and not be sliced + obj_sliceable = self._sliceable_classifier(state, objs) + if obj_sliceable: + return ig_obj.states[object_states.Sliced].get_value() + return False + + @staticmethod - def _ig_object_name(ig_obj: "ArticulatedObject") -> str: + def _ig_object_name(ig_obj: "ArticulatedObject") -> List[str]: if isinstance(ig_obj, (URDFObject, RoomFloor)): - return ig_obj.bddl_object_scope + return [ig_obj.bddl_object_scope] + # For multiplexers, only object_id 0 has a bddl_scope + # Note: internally, the multiplexer gets its attributes + # from the current_index + if isinstance(ig_obj, ObjectMultiplexer): + idx = ig_obj.current_index + if idx == 0: + return [ig_obj.bddl_object_scope] + else: + ig_obj.set_selection(0) + name = ig_obj.bddl_object_scope + ig_obj.set_selection(1) + return [name+'_left', name+'_right'] # Robot does not have a field "bddl_object_scope", so we define # its name manually. assert isinstance(ig_obj, BRBody) - return "agent" + return ["agent"] @staticmethod def _bddl_predicate_arity(bddl_predicate: "bddl.AtomicFormula") -> int: diff --git a/predicators/ground_truth_nsrts.py b/predicators/ground_truth_nsrts.py index fedaf8dfe3..84228fd81a 100644 --- a/predicators/ground_truth_nsrts.py +++ b/predicators/ground_truth_nsrts.py @@ -9,7 +9,8 @@ from predicators.behavior_utils.behavior_utils import OPENABLE_OBJECT_TYPES, \ PICK_PLACE_OBJECT_TYPES, PLACE_INTO_SURFACE_OBJECT_TYPES, \ - PLACE_ONTOP_SURFACE_OBJECT_TYPES, check_hand_end_pose, \ + PLACE_ONTOP_SURFACE_OBJECT_TYPES, SLICEABLE_OBJECT_TYPES, \ + SLICER_OBJECT_TYPES, check_hand_end_pose, \ check_nav_end_pose, load_checkpoint_state from predicators.envs import get_or_create_env from predicators.envs.behavior import BehaviorEnv @@ -2794,6 +2795,7 @@ def _get_predicate(base_pred_name: str, op_name_count_open = itertools.count() op_name_count_close = itertools.count() op_name_count_place_inside = itertools.count() + op_name_count_slice = itertools.count() # Dummy sampler definition. Useful for open and close. def dummy_param_sampler(state: State, goal: Set[GroundAtom], @@ -3095,8 +3097,9 @@ def place_inside_obj_pos_sampler( targ_holding = _get_lifted_atom("holding", [target_obj]) ontop = _get_lifted_atom("ontop", [target_obj, surf_obj]) inside = _get_lifted_atom("inside", [target_obj, surf_obj]) - preconditions_ontop = {handempty, targ_reachable, ontop} - preconditions_inside = {handempty, targ_reachable, inside} + not_sliced = _get_lifted_atom("not-sliced", [target_obj]) + preconditions_ontop = {handempty, targ_reachable, not_sliced, ontop} + preconditions_inside = {handempty, targ_reachable, not_sliced, inside} add_effects = {targ_holding} delete_effects_ontop = {handempty, ontop, targ_reachable} delete_effects_inside = {handempty, inside, targ_reachable} @@ -3330,11 +3333,48 @@ def place_inside_obj_pos_sampler( ), ) nsrts.add(not_openable_nsrt) + elif base_option_name == "Slice": + assert len(option_arg_type_names) == 1 + slice_obj_type_name = option_arg_type_names[0] + slice_obj_type = type_name_to_type[slice_obj_type_name] + slice_obj = Variable("?obj", slice_obj_type) + # We don't need an NSRT to slice objects that are not + # sliceable. + if slice_obj_type.name not in SLICEABLE_OBJECT_TYPES: + continue + for held_obj_types in sorted(env.task_relevant_types): + # If the held object cannot slice, then we don't need + # to make an NSRT for it + if held_obj_types.name not in SLICER_OBJECT_TYPES: + continue + held_obj = Variable("?held", held_obj_types) + parameters = [held_obj, slice_obj] + option_vars = [slice_obj] + preconditions = { + _get_lifted_atom("reachable", [slice_obj]), + _get_lifted_atom("holding", [held_obj]), + _get_lifted_atom("not-sliced", [slice_obj]), + _get_lifted_atom("sliceable", [slice_obj]), + } + add_effects = {_get_lifted_atom("sliced", [slice_obj])} + delete_effects = {_get_lifted_atom("not-sliced", [slice_obj])} + nsrt = NSRT( + f"{option.name}-{next(op_name_count_slice)}", parameters, + preconditions, add_effects, delete_effects, set(), option, + option_vars, lambda s, g, r, o: dummy_param_sampler( + s, + g, + r, + [ + env.object_to_ig_object(o_i) + if isinstance(o_i, Object) else o_i for o_i in o + ], + )) + nsrts.add(nsrt) else: raise ValueError( f"Unexpected base option name: {base_option_name}") - return nsrts