From 15c741d593f40a92503a5533ce8248d0a2cbfacc Mon Sep 17 00:00:00 2001 From: Ashay Athalye Date: Wed, 18 Sep 2024 20:50:26 -0400 Subject: [PATCH] Progress towards testing cleaning mess demo. --- predicators/envs/spot_env.py | 103 +++++++++++-- predicators/perception/spot_perceiver.py | 139 ++++++++++++------ predicators/pretrained_model_interface.py | 4 +- .../spot_utils/perception/spot_cameras.py | 6 +- 4 files changed, 192 insertions(+), 60 deletions(-) diff --git a/predicators/envs/spot_env.py b/predicators/envs/spot_env.py index 7ea7fd0cc..e469031e6 100644 --- a/predicators/envs/spot_env.py +++ b/predicators/envs/spot_env.py @@ -1444,8 +1444,8 @@ def _get_sweeping_surface_for_container(container: Object, _on_classifier) _TopAbove = Predicate("TopAbove", [_base_object_type, _base_object_type], _top_above_classifier) -_Inside = Predicate("Inside", [_movable_object_type, _container_type], - _inside_classifier) +# _Inside = Predicate("Inside", [_movable_object_type, _container_type], +# _inside_classifier) _FitsInXY = Predicate("FitsInXY", [_movable_object_type, _base_object_type], _fits_in_xy_classifier) # NOTE: use this predicate instead if you want to disable inside checking. @@ -1510,6 +1510,18 @@ def _get_vlm_query_str(pred_name: str, objects: Sequence[Object]) -> str: _Stained = utils.create_vlm_predicate( "Stained", [_movable_object_type], lambda o: _get_vlm_query_str("Stained", o)) +_Messy= utils.create_vlm_predicate( + "Messy", [_movable_object_type], + lambda o: _get_vlm_query_str("Messy", o)) + +# long = "Is the dustpan oriented such that a single sweeping motion with a broom would move the mess into the dustpan?" +# long = "Touching(dustpan, mess)" +_Touching = utils.create_vlm_predicate( + "Touching", [_movable_object_type, _movable_object_type], + lambda o: _get_vlm_query_str("Touching(dustpan, mess)", o)) +_Inside = utils.create_vlm_predicate( + "Inside", [_movable_object_type, _movable_object_type], + lambda o: _get_vlm_query_str("Inside(mess, dustpan)", o)) _ALL_PREDICATES = { _NEq, _On, _TopAbove, _Inside, _NotInsideAnyContainer, _FitsInXY, @@ -1517,7 +1529,7 @@ def _get_vlm_query_str(pred_name: str, objects: Sequence[Object]) -> str: _Blocking, _NotBlocked, _ContainerReadyForSweeping, _IsPlaceable, _IsNotPlaceable, _IsSweeper, _HasFlatTopSurface, _RobotReadyForSweeping, _IsSemanticallyGreaterThan, _VLMOn, _Upright, _Toasted, _VLMIn, _Open, - _Stained + _Stained, _Messy, _Touching, _Inside } _NONPERCEPT_PREDICATES: Set[Predicate] = set() @@ -2451,7 +2463,7 @@ class VLMTestEnv(SpotRearrangementEnv): def predicates(self) -> Set[Predicate]: # return set(p for p in _ALL_PREDICATES if p.name in ["VLMOn", "Holding", "HandEmpty", "Pourable", "Toasted", "VLMIn", "Open"]) return set(p for p in _ALL_PREDICATES - if p.name in ["VLMOn", "Holding", "HandEmpty"]) + if p.name in ["Holding", "HandEmpty", "NotHolding", "Touching", "Inside"]) @property def goal_predicates(self) -> Set[Predicate]: @@ -2470,11 +2482,13 @@ def _detection_id_to_obj(self) -> Dict[ObjectDetectionID, Object]: detection_id_to_obj: Dict[ObjectDetectionID, Object] = {} objects = { - Object("pan", _movable_object_type), - Object("cup", _movable_object_type), - Object("chair", _movable_object_type), - Object("bowl", _movable_object_type), - Object("table", _movable_object_type), + # Object("pan", _movable_object_type), + # Object("cup", _movable_object_type), + # Object("chair", _movable_object_type), + # Object("bowl", _movable_object_type), + # Object("table", _movable_object_type), + Object("wrappers", _movable_object_type), + Object("dustpan", _movable_object_type), } for o in objects: detection_id = LanguageObjectDetectionID(o.name) @@ -2517,6 +2531,74 @@ def _create_operators(self) -> Iterator[STRIPSOperator]: ignore_effs: Set[LiftedAtom] = set() yield STRIPSOperator("Place", parameters, preconds, add_effs, del_effs, ignore_effs) + + + ##########################################3 + # Pick(robot, dustpan) + robot = Variable("?robot", _robot_type) + dustpan = Variable("?dustpan", _movable_object_type) + parameters = [robot, dustpan] + preconds: Set[LiftedAtom] = { + LiftedAtom(_HandEmpty, [robot]), + LiftedAtom(_NotHolding, [robot, dustpan]), + } + add_effs: Set[LiftedAtom] = {LiftedAtom(_Holding, [robot, dustpan])} + del_effs: Set[LiftedAtom] = { + LiftedAtom(_HandEmpty, [robot]), + LiftedAtom(_NotHolding, [robot, dustpan]), + } + ignore_effs: Set[LiftedAtom] = set() + yield STRIPSOperator("Pick", parameters, preconds, add_effs, del_effs, + ignore_effs) + + # Place(robot, dustpan, mess) + robot = Variable("?robot", _robot_type) + dustpan = Variable("?dustpan", _movable_object_type) + mess = Variable("?mess", _movable_object_type) + parameters = [robot, dustpan, mess] + preconds: Set[LiftedAtom] = {LiftedAtom(_Holding, [robot, dustpan])} + add_effs: Set[LiftedAtom] = { + LiftedAtom(_HandEmpty, [robot]), + LiftedAtom(_NotHolding, [robot, dustpan]), + LiftedAtom(_Touching, [dustpan, mess]) + } + del_effs: Set[LiftedAtom] = {LiftedAtom(_Holding, [robot, dustpan])} + ignore_effs: Set[LiftedAtom] = set() + yield STRIPSOperator("Place", parameters, preconds, add_effs, del_effs, + ignore_effs) + + # Pick(robot, broom) + robot = Variable("?robot", _robot_type) + broom = Variable("?broom", _movable_object_type) + parameters = [robot, broom] + preconds: Set[LiftedAtom] = { + LiftedAtom(_HandEmpty, [robot]), + LiftedAtom(_NotHolding, [robot, broom]), + } + add_effs: Set[LiftedAtom] = {LiftedAtom(_Holding, [robot, broom])} + del_effs: Set[LiftedAtom] = { + LiftedAtom(_HandEmpty, [robot]), + LiftedAtom(_NotHolding, [robot, broom]), + } + ignore_effs: Set[LiftedAtom] = set() + yield STRIPSOperator("Pick", parameters, preconds, add_effs, del_effs, + ignore_effs) + + # Sweep(robot, broom, mess, dustpan) + robot = Variable("?robot", _robot_type) + broom = Variable("?broom", _movable_object_type) + mess = Variable("?mess", _movable_object_type) + dustpan = Variable("?dustpan", _movable_object_type) + parameters = [robot, broom, mess, dustpan] + preconds: Set[LiftedAtom] = { + LiftedAtom(_Holding, [robot, broom]), + LiftedAtom(_Touching, [dustpan, mess]) + } + add_effs: Set[LiftedAtom] = {LiftedAtom(_Inside, [mess, dustpan])} + del_effs: Set[LiftedAtom] = set() + ignore_effs: Set[LiftedAtom] = set() + yield STRIPSOperator("Sweep", parameters, preconds, add_effs, del_effs, + ignore_effs) # def _generate_train_tasks(self) -> List[EnvironmentTask]: # goal = self._generate_goal_description() # currently just one goal @@ -2639,7 +2721,8 @@ def _actively_construct_env_task(self) -> EnvironmentTask: return task def _generate_goal_description(self) -> GoalDescription: - return "put the cup in the pan" + # return "put the cup in the pan" + return "put the mess in the dustpan" def reset(self, train_or_test: str, task_idx: int) -> Observation: prompt = f"Please set up {train_or_test} task {task_idx}!" diff --git a/predicators/perception/spot_perceiver.py b/predicators/perception/spot_perceiver.py index 3f908130f..d60b007a5 100644 --- a/predicators/perception/spot_perceiver.py +++ b/predicators/perception/spot_perceiver.py @@ -644,15 +644,21 @@ def _create_goal(self, state: State, pred_name_to_pred = {p.name: p for p in self._curr_env.predicates} VLMOn = pred_name_to_pred["VLMOn"] HandEmpty = pred_name_to_pred["HandEmpty"] + + if goal_description == "put the cup in the pan": robot = Object("robot", _robot_type) cup = Object("cup", _movable_object_type) pan = Object("pan", _container_type) goal = { GroundAtom(HandEmpty, [robot]), - GroundAtom(VLMOn, [cup, pan]) + # GroundAtom(VLMOn, [cup, pan]) } return goal + + if goal_description == "put the mess in the dustpan": + + raise NotImplementedError("Unrecognized goal description") def update_perceiver_with_action(self, action: Action) -> None: @@ -765,6 +771,7 @@ def step(self, observation: Observation) -> State: state_copy = self._curr_state.copy() print(f"Right before abstract state, skill in obs: {observation.executed_skill}") abstract_state = utils.abstract(state_copy, preds) + import pdb; pdb.set_trace() self._curr_state.simulator_state["abstract_state"] = abstract_state # Compute all the VLM atoms. `utils.abstract()` only returns the ones that # are True. The remaining ones are the ones that are False. @@ -792,9 +799,11 @@ def _create_state(self) -> State: return DefaultState # Build the continuous part of the state. assert self._robot is not None - table = Object("table", _movable_object_type) - cup = Object("cup", _movable_object_type) - pan = Object("pan", _container_type) + # table = Object("table", _movable_object_type) + # cup = Object("cup", _movable_object_type) + # pan = Object("pan", _container_type) + wrappers = Object("wrappers", _movable_object_type) + dustpan = Object("dustpan", _movable_object_type) # bread = Object("bread", _movable_object_type) # toaster = Object("toaster", _immovable_object_type) # microwave = Object("microwave", _movable_object_type) @@ -810,46 +819,46 @@ def _create_state(self) -> State: "qy": 0, "qz": 0, }, - table: { - "x": 0, - "y": 0, - "z": 0, - "qw": 0, - "qx": 0, - "qy": 0, - "qz": 0, - "shape": 0, - "height": 0, - "width" : 0, - "length": 0, - "object_id": 0, - "placeable": 1, - "held": 0, - "lost": 0, - "in_hand_view": 0, - "in_view": 1, - "is_sweeper": 0 - }, - cup: { - "x": 0, - "y": 0, - "z": 0, - "qw": 0, - "qx": 0, - "qy": 0, - "qz": 0, - "shape": 0, - "height": 0, - "width": 0, - "length": 0, - "object_id": 1, - "placeable": 1, - "held": 0, - "lost": 0, - "in_hand_view": 0, - "in_view": 1, - "is_sweeper": 0 - }, + # table: { + # "x": 0, + # "y": 0, + # "z": 0, + # "qw": 0, + # "qx": 0, + # "qy": 0, + # "qz": 0, + # "shape": 0, + # "height": 0, + # "width" : 0, + # "length": 0, + # "object_id": 0, + # "placeable": 1, + # "held": 0, + # "lost": 0, + # "in_hand_view": 0, + # "in_view": 1, + # "is_sweeper": 0 + # }, + # cup: { + # "x": 0, + # "y": 0, + # "z": 0, + # "qw": 0, + # "qx": 0, + # "qy": 0, + # "qz": 0, + # "shape": 0, + # "height": 0, + # "width": 0, + # "length": 0, + # "object_id": 1, + # "placeable": 1, + # "held": 0, + # "lost": 0, + # "in_hand_view": 0, + # "in_view": 1, + # "is_sweeper": 0 + # }, # napkin: { # "x": 0, # "y": 0, @@ -925,7 +934,47 @@ def _create_state(self) -> State: # "object_id": 1, # "flat_top_surface": 1 # }, - pan: { + # pan: { + # "x": 0, + # "y": 0, + # "z": 0, + # "qw": 0, + # "qx": 0, + # "qy": 0, + # "qz": 0, + # "shape": 0, + # "height": 0, + # "width" : 0, + # "length": 0, + # "object_id": 2, + # "placeable": 1, + # "held": 0, + # "lost": 0, + # "in_hand_view": 0, + # "in_view": 1, + # "is_sweeper": 0 + # }, + wrappers: { + "x": 0, + "y": 0, + "z": 0, + "qw": 0, + "qx": 0, + "qy": 0, + "qz": 0, + "shape": 0, + "height": 0, + "width" : 0, + "length": 0, + "object_id": 2, + "placeable": 1, + "held": 0, + "lost": 0, + "in_hand_view": 0, + "in_view": 1, + "is_sweeper": 0 + }, + dustpan: { "x": 0, "y": 0, "z": 0, diff --git a/predicators/pretrained_model_interface.py b/predicators/pretrained_model_interface.py index 609d5ee90..50fa78faf 100644 --- a/predicators/pretrained_model_interface.py +++ b/predicators/pretrained_model_interface.py @@ -171,8 +171,8 @@ def set_openai_key(self, key: Optional[str] = None) -> None: assert "OPENAI_API_KEY" in os.environ key = os.environ["OPENAI_API_KEY"] - @retry(wait=wait_random_exponential(min=1, max=60), - stop=stop_after_attempt(10)) + # @retry(wait=wait_random_exponential(min=1, max=60), + # stop=stop_after_attempt(10)) def call_openai_api(self, messages: list, model: str = "gpt-4", diff --git a/predicators/spot_utils/perception/spot_cameras.py b/predicators/spot_utils/perception/spot_cameras.py index a1a9780e9..9f85bcc83 100644 --- a/predicators/spot_utils/perception/spot_cameras.py +++ b/predicators/spot_utils/perception/spot_cameras.py @@ -24,10 +24,10 @@ } RGB_TO_DEPTH_CAMERAS = { # "hand_color_image": "hand_depth_in_hand_color_frame", - "left_fisheye_image": "left_depth_in_visual_frame", + # "left_fisheye_image": "left_depth_in_visual_frame", # "right_fisheye_image": "right_depth_in_visual_frame", - # "frontleft_fisheye_image": "frontleft_depth_in_visual_frame", - "frontright_fisheye_image": "frontright_depth_in_visual_frame", + "frontleft_fisheye_image": "frontleft_depth_in_visual_frame", + # "frontright_fisheye_image": "frontright_depth_in_visual_frame", # "back_fisheye_image": "back_depth_in_visual_frame" }