diff --git a/predicators/behavior_utils/behavior_utils.py b/predicators/behavior_utils/behavior_utils.py index 3dc4b5c517..cc86134d8e 100644 --- a/predicators/behavior_utils/behavior_utils.py +++ b/predicators/behavior_utils/behavior_utils.py @@ -37,7 +37,8 @@ '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', 'bath_towel', 'bowl', + 'cup', 'sink' } PICK_PLACE_OBJECT_TYPES = { 'mineral_water', 'oatmeal', 'blueberry', 'headset', 'jug', 'flank', @@ -185,6 +186,15 @@ } +CLEANING_OBJECT_TYPES = { + 'toothbrush', 'towel', 'dinner_napkin', 'paper_towel', 'dishtowel', 'broom', + 'vacuum', 'rag', 'carpet_sweeper', 'hand_towel', 'scraper', 'bath_towel', 'eraser', + 'dustcloth', 'scrub_brush' +} + +DUSTYABLE_OBJECT_TYPES = {'tabletop', 'face', 'dumbbell', 'corkscrew', 'terry', 'circle', 'fur', 'coaster', 'gauze', 'cotton', 'chock', 'trap', 'compact_disk', 'cap', 'stake', 'converter', 'peripheral', 'slide_fastener', 'headset', 'jug', 'baseball', 'window', 'straightener', 'computer_game', 'toaster', 'data_input_device', 'collar', 'accelerator', 'shoe', 'truck', 'remote_control', 'apron', 'nozzle', 'dander', 'bench', 'washcloth', 'ipod', 'journal', 'fork', 'outerwear', 'cherry', 'ammunition', 'work', 'quilt', 'wheel', 'tin', 'plywood', 'instrument', 'bidet', 'bag', 'bowl', 'squeegee', 'water_scooter', 'washer', 'panel', 'basket', 'motor_vehicle', 'mixer', 'hammer', 'suit', 'carabiner', 'sock', 'slipper', 'screen', 'boat', 'bath_linen', 'diary', 'drain', 'armoire', 'pegboard', 'dishwasher', 'vent', 'golf_equipment', 'wrench', 'personal_computer', 'straight_chair', 'suede_cloth', 'semiconductor_device', 'ginger', 'machine', 'backpack', 'tank', 'computer_circuit', 'helmet', 'magnetic_disk', 'ribbon', 'arrangement', 'pitcher', 'table_knife', 'disk', 'folding_chair', 'circuit', 'pump', 'toweling', 'solid_figure', 'drawstring_bag', 'rule', 'hamper', 'caddy', 'cheeseboard', 'console_table', 'headlight', 'medical_instrument', 'can', 'highchair', 'musical_instrument', 'canvas', 'velcro', 'mascara', 'loudspeaker', 'hacksaw', 'newspaper', 'packet', 'knot', 'jersey', 'shaver', 'screw', 'marker', 'floor_lamp', 'carriage', 'act', 'tulle', 'screwdriver', 'greatcoat', 'softball', 'cotter', 'parlor_game', 'skewer', 'tack', 'eiderdown', 'highlighter', 'candle', 'ring', 'bell', 'banana', 'grate', 'coffeepot', 'crib', 'flashlight', 'shrapnel', 'toothbrush', 'inverter', 'shelf', 'regulator', 'carving_knife', 'psychological_feature', 'camcorder', 'spring', 'razor', 'ceramic_ware', 'indentation', 'post', 'plumbing', 'handle', 'caster', 'staple', 'buckle', 'frill', 'tumbler', 'steamer', 'cradle', 'earmuff', 'digital_camera', 'swab', 'shield', 'hose', 'caliper', 'activity', 'mill', 'acoustic_device', 'room', 'carrot', 'drumstick', 'thimble', 'ladder', 'thing', 'snake', 'wedge', 'stairway', 'voltmeter', 'heater', 'push_button', 'frying_pan', 'photograph', 'hardback', 'basil', 'trouser', 'rail', 'folderal', 'junction', 'dress', 'anchor', 'blade', 'change_of_location', 'chest', 'bolt', 'bracelet', 'photographic_equipment', 'printed_circuit', 'dial', 'stringed_instrument', 'computer', 'clasp', 'dipper', 'cornice', 'uniform', 'vacuum', 'electro-acoustic_transducer', 'clothespin', 'hood', 'plate', 'bannister', 'brocade', 'pincer', 'pool_table', 'eyeshadow', 'painting', 'footstool', 'bedpost', 'power_tool', 'sifter', 'drum_sander', 'drive', 'blazer', 'layer', 'apple', 'shaft', 'writing', 'towel_rack', 'thermostat', 'microphone', 'rocking_chair', 'stool', 'teapot', 'turnbuckle', 'toothpick', 'stirrer', 'loafer', 'whisk', 'sieve', 'frisbee', 'movement', 'alarm', 'fan', 'dartboard', 'spatula', 'filter', 'gauge', 'bobbin', 'countertop', 'electrical_converter', 'heating_element', 'sink', 'stiletto', 'porcelain', 'product', 'roller', 'pack', 'circuit_breaker', 'oxford', 'projection', 'step_ladder', 'shoebox', 'protective_garment', 'antenna', 'worktable', 'sphere', 'armor_plate', 'box', 'adapter', 'disk_drive', 'digital_display', 'repeater', 'mantel', 'wine_bottle', 'baggage', 'pocketknife', 'diskette', 'basketball_equipment', 'shear', 'table', 'table_lamp', 'file', 'mouse', 'door', 'receiver', 'solid', 'cylinder', 'breakfast_table', 'radio_receiver', 'cinder', 'brick', 'crock', 'spout', 'blanket', 'background', 'watchband', 'tile', 'battery', 'handlebar', 'floor_cover', 'cord', 'bead', 'television_equipment', 'tablet', 'writing_board', 'faceplate', 'appendage', 'broomstick', 'coffee_table', 'desk', 'bale', 'model', 'hoop', 'belt', 'trimmer', 'recreational_vehicle', 'laundry', 'putter', 'drafting_instrument', 'stopcock', 'module', 'pencil_box', 'jewelry', 'yardstick', 'dolly', 'wastepaper_basket', 'dagger', 'liquid_crystal_display', 'digital_computer', 'well', 'chair', 'foot_rule', 'clothesline', 'set', 'purifier', 'lens', 'cpu_board', 'binder', 'planner', 'bath', 'upholstery', 'resistor', 'cooler', 'plate_glass', 'drill', 'van', 'clamshell', 'brush', 'diode', 'pill', 'drum', 'timer', 'case', 'mat', 'windowsill', 'wall_clock', 'gaming_table', 'wallet', 'composition', 'faucet', 'bathtub', 'air_pump', 'bundle', 'electronic_device', 'clout_nail', 'elastic_device', 'applicator', 'pineapple', 'opener', 'tomato', 'microwave', 'piano', 'nutcracker', 'upright', 'sleeve', 'lighting_fixture', 'pane', 'noisemaker', 'winder', 'pedal', 'broom', 'measuring_stick', 'hubcap', 'bulletin_board', 'canopy', 'pedestal_table', 'guard', 'tablespoon', 'sofa', 'ventilation', 'sail', 'floor', 'pencil', 'cringle', 'utility', 'breathing_device', 'silk', 'umbrella', 'laptop', 'boot', 'peach', 'electric_lamp', 'crowbar', 'groundsheet', 'funnel', 'shell', 'fitting', 'tire', 'group', 'nail', 'mousetrap', 'knife', 'tray', 'cellular_telephone', 'firebox', 'shoulder_bag', 'inflater', 'movable_barrier', 'baseboard', 'chest_of_drawers', 'piston', 'armchair', 'duffel_bag', 'speedometer', 'sculpture', 'baby_bed', 'sled', 'paper_fastener', 'necklace', 'happening', 'mattress', 'ceramic', 'wicker', 'scanner', 'sprocket', 'eyeliner', 'craft', 'shade', 'thermometer', 'memory_device', 'rail_fence', 'footboard', 'runner', 'paperweight', 'game', 'crate', 'tablefork', 'recording', 'cabinet', 'television_receiver', 'step', 'storage_space', 'crayon', 'crossbar', 'chaise_longue', 'griddle', 'demitasse', 'document', "plumber's_snake", 'converging_lens', 'lock', 'trailer_truck', 'compressor', 'plastic_art', 'car', 'display_panel', 'earplug', 'golf_club', 'blower', 'sharpener', 'percolator', 'percussion_instrument', 'charger', 'grater', 'vessel', 'cooking_utensil', 'manifold', 'exercise_device', 'blinker', 'reamer', 'rack', 'deep-freeze', 'timepiece', 'machinery', 'workwear', 'cable', 'fire_extinguisher', 'pan', 'floorboard', 'wallboard', 'bookshelf', 'vest', 'pestle', 'side', 'modem', 'gingham', 'display', 'motorcycle', 'written_communication', 'tarpaulin', 'flatware', 'doorknob', 'facsimile', 'dredging_bucket', 'cassette', 'opening', 'bicycle', 'truck_bed', 'plunger', 'light_bulb', 'tent', 'hat', 'dryer', 'protractor', 'library', 'wiring', 'tongs', 'burner', 'blackboard', 'bin', 'portrait', 'strengthener', 'steel', 'hanger', 'lath', 'string', 'walker', 'doll', 'material', 'hinge', 'paper_towel', 'lemon', 'jaw', 'shim', 'glass', 'sawhorse', 'refrigerator', 'shaker', 'monocle', 'work_surface', 'cellophane', 'tinsel', 'jar', 'transducer', 'cupboard', 'windowpane', 'turner', 'barrel', 'stand', 'self-propelled_vehicle', 'bird_feeder', 'lampshade', 'sweatband', 'colander', 'comb', 'caldron', 'range_hood', 'flow', 'brim', 'enamel', 'bust', 'stove', 'knob', 'wall_socket', 'thumbtack', 'embroidery', 'carafe', 'stockpot', 'buffer', 'cream_pitcher', 'headdress', 'chamber', 'palette', 'ink_cartridge', 'radiotelephone', 'connection', 'projectile', 'lamp', 'kettle', 'grandfather_clock', 'tiara', 'clamp', 'book', 'railing', 'hole', 'bow', 'weight', 'basin', 'television_camera', 'medallion', 'molding', 'apparel', 'toilet', 'stopwatch', 'flatbed', 'ski', 'autoclave', 'pouch', 'footwear', 'platter', 'dish_rack', 'dart', 'power_shovel', 'light-emitting_diode', 'passageway', 'jewelled_headdress', 'slat', 'award', 'basketball', 'telephone_receiver', 'drinking_vessel', 'cup', 'doorjamb', 'pendulum', 'eraser', 'scrub_brush', 'iron', 'furnace', 'packaging', 'sandal', 'dustpan', 'fuse', 'calculator', 'knickknack', 'bath_towel', 'plastic_wrap', 'cleaver', 'baby_buggy', 'boiler', 'scale', 'roaster', 'saucepan', 'ashcan', 'barbell', 'board_game', 'mechanical_system', 'cruet', 'crusher', 'shelter', 'audio_system', 'bookend', 'tea_bag', 'mug', 'crank', 'cone', 'tripod', 'attire', 'pepper_mill', 'stairwell', 'chopping_board', 'sports_equipment', 'headboard', 'saucer', 'lego', 'curtain', 'surgical_instrument', 'reflector', 'skirt', 'cutlery', 'orange', 'skeleton', 'tidy', 'baseball_equipment', 'squeezer', 'wrapping', 'bangle', 'power_saw', 'respirator', 'optical_disk', 'duplicator', 'swivel_chair', 'minibike', 'handset', 'edge', 'coupling', 'hand_towel', 'package', 'globe', 'skateboard', 'watch', 'figure', 'measuring_instrument', 'peg', 'computer_keyboard', 'shirt', 'monitor', 'toolbox', 'grill', 'gym_shoe', 'bedroom_furniture', 'sorter', 'videodisk', 'strip', 'hub', 'headband', 'graphic_art', 'jigsaw', 'football', 'apparatus', 'album', 'pipe', 'flight', 'accessory', 'bookcase', 'magnet', 'facility', 'webbing', 'scrapbook', 'cartridge', 'wreath', 'mirror', 'slate', 'canister', 'socket', 'notebook', 'straight_pin', 'receptacle', 'mask', 'laminate', 'guitar', 'motorboat', 'meter', 'folder', 'intake', 'rope', 'bucket', 'keyboard', 'coatrack', 'bit', 'optical_device', 'earphone', 'puppet', 'deck', 'scantling', 'chopstick', 'chisel', 'briefcase', 'bed', 'stapler', 'camera', 'drawer', 'hook', 'stairs', 'coffee_maker', 'gear', 'electronic_equipment', 'gourd', 'reproducer', 'pole', 'electric_refrigerator', 'likeness', 'kit', 'webcam', 'mural', 'printer', 'potato', 'treadmill', 'necktie', 'probe', 'backing', 'wardrobe', 'weaponry', 'lamination', 'collage', 'generator', 'bulldog_clip', 'white_goods', 'office_furniture', 'vase', 'carryall', 'weapon', 'fluorescent', 'valve', 'hygrometer', 'jamb', 'pulley', 'blind', 'windshield', 'carton', 'gown', 'sill', 'coffee_cup', 'circuit_board', 'rotating_mechanism', 'paintbrush', 'lumber', 'sunhat', 'fountain', 'sunglass', 'picture', 'component', 'trophy', 'bracket', 'cloche', 'garbage', 'lid', 'cashmere', 'seat', 'carpet_pad', 'topper', 'sequin', 'mail', 'telephone', 'portable_computer', 'blender', 'pick', 'jewel', 'diversion', 'rug', 'clipboard', 'doormat', 'toasting_fork', 'dressing_table', 'bottle', 'concave_shape', 'art', 'teacup', 'formalwear', 'easel', 'pot', 'router', 'slab', 'notch', 'dinner_jacket', 'capacitor', 'mallet', 'hairbrush', 'latch', 'odometer', 'paste-up', 'spoon', 'novel', 'clock', 'front', 'lantern', 'teaspoon', 'event', 'horn', 'frame', 'strainer', 'pendulum_clock', 'magazine', 'signaling_device', 'paperback_book', 'flower_arrangement', 'earring', 'keyboard_instrument', 'soup_ladle', 'ratchet', 'jean', 'teddy', 'doorframe', 'bottle_opener', 'board', 'neckwear', 'siren', 'balloon', 'stereo', 'goblet', 'joint'} +DUSTYABLE_OBJECT_TYPES.add('bottom_cabinet_no_top') # somehow missed by parsing script + 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/option_model_fns.py b/predicators/behavior_utils/option_model_fns.py index f73b08761e..12404f7673 100644 --- a/predicators/behavior_utils/option_model_fns.py +++ b/predicators/behavior_utils/option_model_fns.py @@ -268,3 +268,27 @@ def placeInsideObjectOptionModel(_init_state: State, env.step(np.zeros(env.action_space.shape)) return placeInsideObjectOptionModel + +def create_clean_dusty_option_model( + plan: List[List[float]], _original_orientation: List[List[float]], + obj_to_clean: "URDFObject") -> Callable[[State, "BehaviorEnv"], None]: + """Instantiates and returns an clean dusty option model given a dummy plan.""" + del plan + + def cleanDustyObjectOptionModel(_init_state: State, env: "BehaviorEnv") -> None: + logging.info(f"PRIMITIVE: Attempting to clean {obj_to_clean.name}") + if np.linalg.norm( + np.array(obj_to_clean.get_position()) - + np.array(env.robots[0].get_position())) < 2: + if hasattr(obj_to_clean, + "states") and object_states.Dusty in obj_to_clean.states: + obj_to_clean.states[object_states.Dusty].set_value(False) + else: + logging.info("PRIMITIVE clean failed, cannot be cleaned") + else: + logging.info("PRIMITIVE clean failed, too far") + obj_to_clean.force_wakeup() + # Step the simulator to update visuals. + env.step(np.zeros(env.action_space.shape)) + + return cleanDustyObjectOptionModel diff --git a/predicators/envs/behavior.py b/predicators/envs/behavior.py index 804caabe99..b4032e97e1 100644 --- a/predicators/envs/behavior.py +++ b/predicators/envs/behavior.py @@ -47,7 +47,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_clean_dusty_option_model from predicators.envs import BaseEnv from predicators.settings import CFG from predicators.structs import Action, Array, GroundAtom, Object, \ @@ -143,7 +144,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_clean_dusty_option_model ] # name, planner_fn, option_policy_fn, option_model_fn, @@ -161,6 +163,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)), + ("CleanDusty", 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, @@ -331,10 +335,12 @@ def _get_task_goal(self) -> Set[GroundAtom]: # Since our implementation of SeSamE assumes positive preconditions # and goals, we must parse these into positive expressions. if head_expr.terms[0] == 'not': - # Currently, the only goals that include 'not' are those that - # include 'not open' statements, so turn these into 'closed'. - assert head_expr.terms[1] == 'open' - bddl_name = 'closed' + if head_expr.terms[1] == 'open': + bddl_name = 'closed' + elif head_expr.terms[1] == 'dusty': + bddl_name = 'not-dusty' + else: + raise ValueError('Only open and dusty support negation in goals') obj_start_idx = 2 else: bddl_name = head_expr.terms[0] # untyped @@ -408,6 +414,10 @@ def predicates(self) -> Set[Predicate]: ("openable", self._openable_classifier, 1), ("not-openable", self._not_openable_classifier, 1), ("closed", self._closed_classifier, 1), + ("cleaner", self._cleaner_classifier, 1), + ("dustyable", self._dustyable_classifier, 1), + ("dusty", self._dusty_classifier, 1), + ("not-dusty", self._not_dusty_classifier, 1), ] for name, classifier, arity in custom_predicate_specs: @@ -748,6 +758,48 @@ def _closed_classifier(self, state: State, objs: Sequence[Object]) -> bool: return not ig_obj.states[object_states.Open].get_value() return False + def _cleaner_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_cleaner = hasattr( + ig_obj, "states") and object_states.CleaningTool in ig_obj.states + return obj_cleaner + + def _dustyable_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_dustyable = hasattr( + ig_obj, "states") and object_states.Dusty in ig_obj.states + return obj_dustyable + + def _dusty_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_dustyable = self._dustyable_classifier(state, objs) + if obj_dustyable: + return ig_obj.states[object_states.Dusty].get_value() + return False + + def _not_dusty_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_dustyable = self._dustyable_classifier(state, objs) + if obj_dustyable: + return not ig_obj.states[object_states.Dusty].get_value() + return False + @staticmethod def _ig_object_name(ig_obj: "ArticulatedObject") -> str: if isinstance(ig_obj, (URDFObject, RoomFloor)): diff --git a/predicators/ground_truth_nsrts.py b/predicators/ground_truth_nsrts.py index fedaf8dfe3..e54f47145c 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, CLEANING_OBJECT_TYPES, \ + DUSTYABLE_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_clean_dusty = itertools.count() # Dummy sampler definition. Useful for open and close. def dummy_param_sampler(state: State, goal: Set[GroundAtom], @@ -3330,7 +3332,44 @@ def place_inside_obj_pos_sampler( ), ) nsrts.add(not_openable_nsrt) - + elif base_option_name == "CleanDusty": + assert len(option_arg_type_names) == 1 + clean_obj_type_name = option_arg_type_names[0] + clean_obj_type = type_name_to_type[clean_obj_type_name] + clean_obj = Variable("?obj", clean_obj_type) + # We don't need an NSRT to close objects that are not + # openable. + if clean_obj_type.name not in DUSTYABLE_OBJECT_TYPES: + continue + for held_obj_types in sorted(env.task_relevant_types): + if held_obj_types.name not in CLEANING_OBJECT_TYPES or \ + held_obj_types.name == clean_obj_type.name: + continue + held_obj = Variable("?held", held_obj_types) + parameters = [held_obj, clean_obj] + option_vars = [clean_obj] + preconditions = { + _get_lifted_atom("reachable", [clean_obj]), + _get_lifted_atom("holding", [held_obj]), + _get_lifted_atom("dusty", [clean_obj]), + _get_lifted_atom("dustyable", [clean_obj]), + _get_lifted_atom("cleaner", [held_obj]) + } + add_effects = {_get_lifted_atom("not-dusty", [clean_obj])} + delete_effects = {_get_lifted_atom("dusty", [clean_obj])} + nsrt = NSRT( + f"{option.name}-{next(op_name_count_close)}", 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}")