diff --git a/predicators/approaches/bilevel_planning_approach.py b/predicators/approaches/bilevel_planning_approach.py index efa86fa2c..5e8aa815f 100644 --- a/predicators/approaches/bilevel_planning_approach.py +++ b/predicators/approaches/bilevel_planning_approach.py @@ -65,7 +65,6 @@ def _solve(self, task: Task, timeout: int) -> Callable[[State], Action]: if self._plan_without_sim: nsrt_plan, atoms_seq, metrics = self._run_task_plan( task, nsrts, preds, timeout, seed) - import pdb; pdb.set_trace() self._last_nsrt_plan = nsrt_plan self._last_atoms_seq = atoms_seq policy = utils.nsrt_plan_to_greedy_policy(nsrt_plan, task.goal, diff --git a/predicators/envs/spot_env.py b/predicators/envs/spot_env.py index 0748e5d09..07fe37d8e 100644 --- a/predicators/envs/spot_env.py +++ b/predicators/envs/spot_env.py @@ -40,9 +40,9 @@ navigate_to_absolute_pose from predicators.spot_utils.skills.spot_stow_arm import stow_arm from predicators.spot_utils.spot_localization import SpotLocalizer -from predicators.spot_utils.utils import _base_object_type, _container_type, \ - _immovable_object_type, _movable_object_type, _robot_type, \ - _broom_type, _dustpan_type, _wrappers_type, \ +from predicators.spot_utils.utils import _base_object_type, _broom_type, \ + _container_type, _dustpan_type, _immovable_object_type, \ + _movable_object_type, _robot_type, _wrappers_type, \ construct_state_given_pbrspot, get_allowed_map_regions, \ get_graph_nav_dir, get_robot_gripper_open_percentage, get_spot_home_pose, \ load_spot_metadata, object_to_top_down_geom, update_pbrspot_given_state, \ @@ -108,7 +108,8 @@ class _TruncatedSpotObservation: # nonpercept_atoms: Set[GroundAtom] # nonpercept_predicates: Set[Predicate] # Object detections per camera in self.rgbd_images. - object_detections_per_camera: Dict[str, List[Tuple[ObjectDetectionID, SegmentedBoundingBox]]] + object_detections_per_camera: Dict[str, List[Tuple[ObjectDetectionID, + SegmentedBoundingBox]]] executed_skill: Optional[_Option] = None @@ -1511,24 +1512,22 @@ 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)) +_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", [_dustpan_type, _wrappers_type], lambda o: _get_vlm_query_str("Touching", o)) -_Inside = utils.create_vlm_predicate( - "Inside", [_wrappers_type, _dustpan_type], - lambda o: _get_vlm_query_str("Inside", o)) +_Inside = utils.create_vlm_predicate("Inside", [_wrappers_type, _dustpan_type], + lambda o: _get_vlm_query_str("Inside", o)) _ALL_PREDICATES = { - _NEq, _On, _TopAbove, _NotInsideAnyContainer, _FitsInXY, - _HandEmpty, _Holding, _NotHolding, _InHandView, _InView, _Reachable, - _Blocking, _NotBlocked, _ContainerReadyForSweeping, _IsPlaceable, - _IsNotPlaceable, _IsSweeper, _HasFlatTopSurface, _RobotReadyForSweeping, + _NEq, _On, _TopAbove, _NotInsideAnyContainer, _FitsInXY, _HandEmpty, + _Holding, _NotHolding, _InHandView, _InView, _Reachable, _Blocking, + _NotBlocked, _ContainerReadyForSweeping, _IsPlaceable, _IsNotPlaceable, + _IsSweeper, _HasFlatTopSurface, _RobotReadyForSweeping, _IsSemanticallyGreaterThan, _VLMOn, _Upright, _Toasted, _VLMIn, _Open, _Stained, _Messy, _Touching, _Inside } @@ -2463,8 +2462,9 @@ class VLMTestEnv(SpotRearrangementEnv): @property 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 ["Holding", "HandEmpty", "NotHolding", "Touching", "Inside"]) + return set( + p for p in _ALL_PREDICATES if p.name in + ["Holding", "HandEmpty", "NotHolding", "Touching", "Inside"]) @property def goal_predicates(self) -> Set[Predicate]: @@ -2532,7 +2532,6 @@ 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) @@ -2565,9 +2564,9 @@ def _create_operators(self) -> Iterator[STRIPSOperator]: } del_effs: Set[LiftedAtom] = {LiftedAtom(_Holding, [robot, dustpan])} ignore_effs: Set[LiftedAtom] = set() - yield STRIPSOperator("PlaceNextTo", parameters, preconds, add_effs, del_effs, - ignore_effs) - + yield STRIPSOperator("PlaceNextTo", parameters, preconds, add_effs, + del_effs, ignore_effs) + # Pick(robot, broom) robot = Variable("?robot", _robot_type) broom = Variable("?broom", _broom_type) @@ -2584,7 +2583,7 @@ def _create_operators(self) -> Iterator[STRIPSOperator]: ignore_effs: Set[LiftedAtom] = set() yield STRIPSOperator("Pick2", parameters, preconds, add_effs, del_effs, ignore_effs) - + # Sweep(robot, broom, mess, dustpan) robot = Variable("?robot", _robot_type) broom = Variable("?broom", _broom_type) @@ -2601,7 +2600,7 @@ def _create_operators(self) -> Iterator[STRIPSOperator]: ignore_effs: Set[LiftedAtom] = set() yield STRIPSOperator("Sweep", parameters, preconds, add_effs, del_effs, ignore_effs) - + # Place(robot, broom) robot = Variable("?robot", _robot_type) broom = Variable("?broom", _broom_type) @@ -2613,8 +2612,8 @@ def _create_operators(self) -> Iterator[STRIPSOperator]: } del_effs: Set[LiftedAtom] = {LiftedAtom(_Holding, [robot, broom])} ignore_effs: Set[LiftedAtom] = set() - yield STRIPSOperator("PlaceOnFloor", parameters, preconds, add_effs, del_effs, - ignore_effs) + yield STRIPSOperator("PlaceOnFloor", parameters, preconds, add_effs, + del_effs, ignore_effs) # def _generate_train_tasks(self) -> List[EnvironmentTask]: # goal = self._generate_goal_description() # currently just one goal @@ -2646,39 +2645,45 @@ def __init__(self, use_gui: bool = True) -> None: # Create constant objects. self._spot_object = Object("robot", _robot_type) op_to_name = {o.name: o for o in self._create_operators()} - op_names_to_keep = {"Pick1", "PlaceNextTo", "Pick2", "Sweep", "PlaceOnFloor"} + op_names_to_keep = { + "Pick1", "PlaceNextTo", "Pick2", "Sweep", "PlaceOnFloor" + } self._strips_operators = {op_to_name[o] for o in op_names_to_keep} self._train_tasks = [] self._test_tasks = [] - - def detect_objects(self, rgbd_images: Dict[str, RGBDImage]) -> Dict[str, List[Tuple[ObjectDetectionID, SegmentedBoundingBox]]]: + + def detect_objects( + self, rgbd_images: Dict[str, RGBDImage] + ) -> Dict[str, List[Tuple[ObjectDetectionID, SegmentedBoundingBox]]]: object_ids = self._detection_id_to_obj.keys() - object_id_to_img_detections = _query_detic_sam2(object_ids, rgbd_images) + object_id_to_img_detections = _query_detic_sam2( + object_ids, rgbd_images) # This ^ is currently a mapping of object_id -> camera_name -> SegmentedBoundingBox. # We want to do our annotations by camera image, so let's turn this into a - # mapping of camera_name -> object_id -> SegmentedBoundingBox. + # mapping of camera_name -> object_id -> SegmentedBoundingBox. detections = {k: [] for k in rgbd_images.keys()} for object_id, d in object_id_to_img_detections.items(): for camera_name, seg_bb in d.items(): detections[camera_name].append((object_id, seg_bb)) return detections - + def _actively_construct_env_task(self) -> EnvironmentTask: assert self._robot is not None rgbd_images = capture_images_without_context(self._robot) + # Uncomment for debugging # import PIL # imgs = [v.rgb for _, v in rgbd_images.items()] # rot_imgs = [v.rotated_rgb for _, v in rgbd_images.items()] # ex1 = PIL.Image.fromarray(imgs[0]) # ex2 = PIL.Image.fromarray(rot_imgs[0]) - # import pdb; pdb.set_trace() - gripper_open_percentage = get_robot_gripper_open_percentage(self._robot) + # import ipdb; ipdb.set_trace() + gripper_open_percentage = get_robot_gripper_open_percentage( + self._robot) objects_in_view = [] # Perform object detection. object_detections_per_camera = self.detect_objects(rgbd_images) - # artifacts = {"language": {"rgbds": rgbd_images, "object_id_to_img_detections": ret}} # detections_outfile = Path(".") / "object_detection_artifacts.png" # no_detections_outfile = Path(".") / "no_detection_artifacts.png" @@ -2694,7 +2699,7 @@ def _actively_construct_env_task(self) -> EnvironmentTask: # for camera, seg_bb in img_detections.items(): # rgbd = rgbds[camera] # flat_detections.append((rgbd, obj_id, seg_bb)) - + # # For now assume we only have 1 image, front-left. # import pdb; pdb.set_trace() # import PIL @@ -2713,7 +2718,7 @@ def _actively_construct_env_task(self) -> EnvironmentTask: # # font = utils.get_scaled_default_font(draw, 4) # # text_width, text_height = draw.textsize(text, font) # # text_width = draw.textlength(text, font) - # # text_height = font.getsize("hg")[1] + # # text_height = font.getsize("hg")[1] # text_mask = font.getmask(text) # text_width, text_height = text_mask.size # text_bbox = [(x0, y0 - text_height - 2), (x0 + text_width + 2, y0)] @@ -2721,17 +2726,11 @@ def _actively_construct_env_task(self) -> EnvironmentTask: # draw.text((x0 + 1, y0 - text_height - 1), text, fill='white', font=font) # import pdb; pdb.set_trace() - - obs = _TruncatedSpotObservation( - rgbd_images, - set(objects_in_view), - set(), - set(), - self._spot_object, - gripper_open_percentage, - object_detections_per_camera, - None - ) + + obs = _TruncatedSpotObservation(rgbd_images, set(objects_in_view), + set(), set(), self._spot_object, + gripper_open_percentage, + object_detections_per_camera, None) goal_description = self._generate_goal_description() task = EnvironmentTask(obs, goal_description) return task @@ -2777,15 +2776,11 @@ def step(self, action: Action) -> Observation: break logging.info("Invalid input, must be either 'y' or 'n'") return _TruncatedSpotObservation( - self._current_observation.rgbd_images, - self._current_observation.objects_in_view, - set(), - set(), - self._spot_object, - self._current_observation.gripper_open_percentage, - self._current_observation.object_detections_per_camera, - action - ) + self._current_observation.rgbd_images, + self._current_observation.objects_in_view, set(), set(), + self._spot_object, + self._current_observation.gripper_open_percentage, + self._current_observation.object_detections_per_camera, action) # Execute the action in the real environment. Automatically retry # if a retryable error is encountered. @@ -2799,21 +2794,16 @@ def step(self, action: Action) -> Observation: logging.warning("WARNING: the following retryable error " f"was encountered. Trying again.\n{e}") rgbd_images = capture_images_without_context(self._robot) - gripper_open_percentage = get_robot_gripper_open_percentage(self._robot) + gripper_open_percentage = get_robot_gripper_open_percentage( + self._robot) objects_in_view = [] # Perform object detection. object_detections_per_camera = self.detect_objects(rgbd_images) - obs = _TruncatedSpotObservation( - rgbd_images, - set(objects_in_view), - set(), - set(), - self._spot_object, - gripper_open_percentage, - object_detections_per_camera, - action - ) + obs = _TruncatedSpotObservation(rgbd_images, set(objects_in_view), + set(), set(), self._spot_object, + gripper_open_percentage, + object_detections_per_camera, action) return obs diff --git a/predicators/ground_truth_models/spot_env/options.py b/predicators/ground_truth_models/spot_env/options.py index 6942fb702..9deca506e 100644 --- a/predicators/ground_truth_models/spot_env/options.py +++ b/predicators/ground_truth_models/spot_env/options.py @@ -900,9 +900,11 @@ def _move_to_ready_sweep_policy(state: State, memory: Dict, state, memory, objects, params) -def _create_teleop_policy_with_name(name: str) -> Callable[[State, Dict, Sequence[Object], Array], Action]: +def _create_teleop_policy_with_name( + name: str) -> Callable[[State, Dict, Sequence[Object], Array], Action]: + def _teleop_policy(state: State, memory: Dict, objects: Sequence[Object], - params: Array) -> Action: + params: Array) -> Action: nonlocal name del state, memory, params @@ -924,9 +926,10 @@ def _teleop(robot: Robot, lease_client: LeaseClient): sim_fn = lambda _: None sim_fn_args = () name = name - action_extra_info = SpotActionExtraInfo(name, objects, fn, fn_args, sim_fn, - sim_fn_args) + action_extra_info = SpotActionExtraInfo(name, objects, fn, fn_args, + sim_fn, sim_fn_args) return utils.create_spot_env_action(action_extra_info) + return _teleop_policy diff --git a/predicators/perception/spot_perceiver.py b/predicators/perception/spot_perceiver.py index aefbab79c..f0fca3e55 100644 --- a/predicators/perception/spot_perceiver.py +++ b/predicators/perception/spot_perceiver.py @@ -21,13 +21,13 @@ AprilTagObjectDetectionID, KnownStaticObjectDetectionID, \ LanguageObjectDetectionID, ObjectDetectionID, _query_detic_sam2, \ detect_objects, visualize_all_artifacts -from predicators.spot_utils.utils import _container_type, \ - _immovable_object_type, _movable_object_type, _robot_type, \ - _broom_type, _dustpan_type, _wrappers_type, \ - get_allowed_map_regions, load_spot_metadata, object_to_top_down_geom +from predicators.spot_utils.utils import _broom_type, _container_type, \ + _dustpan_type, _immovable_object_type, _movable_object_type, _robot_type, \ + _wrappers_type, get_allowed_map_regions, load_spot_metadata, \ + object_to_top_down_geom from predicators.structs import Action, DefaultState, EnvironmentTask, \ GoalDescription, GroundAtom, Object, Observation, Predicate, \ - SpotActionExtraInfo, State, Task, Video, _Option, VLMPredicate + SpotActionExtraInfo, State, Task, Video, VLMPredicate, _Option class SpotPerceiver(BasePerceiver): @@ -634,7 +634,6 @@ def __init__(self) -> None: # Load static, hard-coded features of objects, like their shapes. # meta = load_spot_metadata() # self._static_object_features = meta.get("static-object-features", {}) - def _create_goal(self, state: State, goal_description: GoalDescription) -> Set[GroundAtom]: @@ -648,7 +647,6 @@ def _create_goal(self, state: State, Holding = pred_name_to_pred["Holding"] 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) @@ -677,7 +675,7 @@ def _create_goal(self, state: State, # GroundAtom(Holding, [robot, dustpan]) # } # return goal - + raise NotImplementedError("Unrecognized goal description") def update_perceiver_with_action(self, action: Action) -> None: @@ -688,21 +686,8 @@ def update_perceiver_with_action(self, action: Action) -> None: self._prev_action = action def reset(self, env_task: EnvironmentTask) -> Task: - # import pdb; pdb.set_trace() - # init_obs = env_task.init_obs - # imgs = init_obs.rgbd_images - # self._robot = init_obs.robot - # state = self._create_state() - # state.simulator_state["images"] = [imgs] - # state.set(self._robot, "gripper_open_percentage", init_obs.gripper_open_percentage) - # self._curr_state = state self._curr_env = get_or_create_env(CFG.env) state = self._create_state() - # state.simulator_state = {} - # state.simulator_state["images"] = [] - # state.simulator_state["state_history"] = [] - # state.simulator_state["skill_history"] = [] - # state.simulator_state["vlm_atoms_history"] = [] self._curr_state = state goal = self._create_goal(state, env_task.goal_description) @@ -715,7 +700,6 @@ def reset(self, env_task: EnvironmentTask) -> Task: return Task(state, goal) def step(self, observation: Observation) -> State: - # import pdb; pdb.set_trace() self._waiting_for_observation = False self._robot = observation.robot img_objects = observation.rgbd_images # RGBDImage objects @@ -731,7 +715,9 @@ def step(self, observation: Observation) -> State: draw = ImageDraw.Draw(pil_imgs[i]) # Annotate with camera name. font = utils.get_scaled_default_font(draw, 4) - _ = utils.add_text_to_draw_img(draw, (0, 0), self.camera_name_to_annotation[camera_name], font) + _ = utils.add_text_to_draw_img( + draw, (0, 0), self.camera_name_to_annotation[camera_name], + font) # Annotate with object detections. detections = object_detections_per_camera[camera_name] for obj_id, seg_bb in detections: @@ -741,10 +727,14 @@ def step(self, observation: Observation) -> State: font = utils.get_scaled_default_font(draw, 3) text_mask = font.getmask(text) text_width, text_height = text_mask.size - text_bbox = [(x0, y0 - 1.5*text_height), (x0 + text_width + 1, y0)] + text_bbox = [(x0, y0 - 1.5 * text_height), + (x0 + text_width + 1, y0)] draw.rectangle(text_bbox, fill='green') - draw.text((x0 + 1, y0 - 1.5*text_height), text, fill='white', font=font) - + draw.text((x0 + 1, y0 - 1.5 * text_height), + text, + fill='white', + font=font) + # import PIL # from PIL import ImageDraw # annotated_pil_imgs = [] @@ -767,16 +757,16 @@ def step(self, observation: Observation) -> State: if "Place" in observation.executed_skill.extra_info.action_name: for obj in observation.executed_skill.extra_info.operator_objects: if not obj.is_instance(_robot_type): - # Turn the held feature off + # Turn the held feature off self._curr_state.set(obj, "held", 0.0) - # import pdb; pdb.set_trace() # This state is a default/empty. We have to set the attributes - # of the objects and set the simulator state properly. + # of the objects and set the simulator state properly. self._curr_state.simulator_state["images"] = annotated_imgs # At the first timestep, these histories will be empty due to self.reset(). # But at every timestep that isn't the first one, they will be non-empty. - self._curr_state.simulator_state["state_history"] = list(self._state_history) + self._curr_state.simulator_state["state_history"] = list( + self._state_history) # We do this here so the call to `utils.abstract()` a few lines later has the skill # that was just run. executed_skill = None @@ -786,31 +776,37 @@ def step(self, observation: Observation) -> State: # Just return the default state return DefaultState executed_skill = observation.executed_skill.get_option() - self._executed_skill_history.append(executed_skill) # None in first timestep. - self._curr_state.simulator_state["skill_history"] = list(self._executed_skill_history) - self._curr_state.simulator_state["vlm_label_history"] = list(self._vlm_label_history) + self._executed_skill_history.append( + executed_skill) # None in first timestep. + self._curr_state.simulator_state["skill_history"] = list( + self._executed_skill_history) + self._curr_state.simulator_state["vlm_label_history"] = list( + self._vlm_label_history) # Add to histories. - # A bit of extra work is required to build the VLM label history. - # We want to keep `utils.abstract()` as straightforward as possible, - # so we'll "rebuild" the VLM labels from the abstract state - # returned by `utils.abstract()`. And since we call this function, + # A bit of extra work is required to build the VLM label history. + # We want to keep `utils.abstract()` as straightforward as possible, + # so we'll "rebuild" the VLM labels from the abstract state + # returned by `utils.abstract()`. And since we call this function, # we might as well store the abstract state as a part of the simulator - # state so that we don't need to recompute it later in the approach or + # state so that we don't need to recompute it later in the approach or # in planning. assert self._curr_env is not None preds = self._curr_env.predicates state_copy = self._curr_state.copy() - print(f"Right before abstract state, skill in obs: {observation.executed_skill}") + 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. - vlm_preds = set(pred for pred in preds if isinstance(pred, VLMPredicate)) + vlm_preds = set(pred for pred in preds + if isinstance(pred, VLMPredicate)) vlm_atoms = set() for pred in vlm_preds: - for choice in utils.get_object_combinations(list(state_copy), pred.types): + for choice in utils.get_object_combinations( + list(state_copy), pred.types): vlm_atoms.add(GroundAtom(pred, choice)) vlm_atoms = sorted(vlm_atoms) reconstructed_all_vlm_responses = [] @@ -998,7 +994,7 @@ def _create_state(self) -> State: "qz": 0, "shape": 0, "height": 0, - "width" : 0, + "width": 0, "length": 0, "object_id": 2, "placeable": 1, @@ -1018,7 +1014,7 @@ def _create_state(self) -> State: "qz": 0, "shape": 0, "height": 0, - "width" : 0, + "width": 0, "length": 0, "object_id": 2, "placeable": 1, @@ -1038,7 +1034,7 @@ def _create_state(self) -> State: "qz": 0, "shape": 0, "height": 0, - "width" : 0, + "width": 0, "length": 0, "object_id": 2, "placeable": 1, diff --git a/predicators/spot_utils/perception/object_detection.py b/predicators/spot_utils/perception/object_detection.py index 6b627a90e..893415036 100644 --- a/predicators/spot_utils/perception/object_detection.py +++ b/predicators/spot_utils/perception/object_detection.py @@ -351,6 +351,7 @@ def _query_detic_sam( return object_id_to_img_detections + def _query_detic_sam2( object_ids: Collection[LanguageObjectDetectionID], rgbds: Dict[str, RGBDImage], @@ -375,11 +376,6 @@ def _query_detic_sam2( classes = sorted(o.language_id for o in object_ids) # Query server, retrying to handle possible wifi issues. - # import pdb; pdb.set_trace() - # imgs = [v.rotated_rgb for _, v in rgbds.items()] - # pil_img = PIL.Image.fromarray(imgs[0]) - # import pdb; pdb.set_trace() - for _ in range(max_server_retries): try: r = requests.post("http://localhost:5550/batch_predict", @@ -449,8 +445,6 @@ def _query_detic_sam2( seg_bb = SegmentedBoundingBox(boxes[best_idx], masks[best_idx], scores[best_idx]) object_id_to_img_detections[obj_id][rgbd.camera_name] = seg_bb - - # import pdb; pdb.set_trace() return object_id_to_img_detections diff --git a/predicators/spot_utils/spot_localization.py b/predicators/spot_utils/spot_localization.py index a85399270..3b8945387 100644 --- a/predicators/spot_utils/spot_localization.py +++ b/predicators/spot_utils/spot_localization.py @@ -57,8 +57,6 @@ def __init__(self, robot: Robot, upload_path: Path, robot_state = get_robot_state(self._robot) z_position = robot_state.kinematic_state.transforms_snapshot.child_to_parent_edge_map[ "gpe"].parent_tform_child.position.z - import pdb - pdb.set_trace() # current_odom_tform_body = get_odom_tform_body( # robot_state.kinematic_state.transforms_snapshot).to_proto() # localization = nav_pb2.Localization() diff --git a/predicators/spot_utils/utils.py b/predicators/spot_utils/utils.py index d4e460767..d9e046a01 100644 --- a/predicators/spot_utils/utils.py +++ b/predicators/spot_utils/utils.py @@ -88,14 +88,14 @@ class _Spot3DShape(Enum): list(_movable_object_type.feature_names), parent=_movable_object_type) _dustpan_type = Type("dustpan", - list(_movable_object_type.feature_names), - parent=_movable_object_type) + list(_movable_object_type.feature_names), + parent=_movable_object_type) _broom_type = Type("broom", - list(_movable_object_type.feature_names), - parent=_movable_object_type) + list(_movable_object_type.feature_names), + parent=_movable_object_type) _wrappers_type = Type("wrappers", - list(_movable_object_type.feature_names), - parent=_movable_object_type) + list(_movable_object_type.feature_names), + parent=_movable_object_type) def get_collision_geoms_for_nav(state: State) -> List[_Geom2D]: diff --git a/predicators/structs.py b/predicators/structs.py index 1fa5dd43d..20e06f0a6 100644 --- a/predicators/structs.py +++ b/predicators/structs.py @@ -430,8 +430,6 @@ def _str(self) -> str: def ground(self, sub: VarToObjSub) -> GroundAtom: """Create a GroundAtom with a given substitution.""" - # if not set(self.variables).issubset(set(sub.keys())): - # import pdb; pdb.set_trace() assert set(self.variables).issubset(set(sub.keys())) return GroundAtom(self.predicate, [sub[v] for v in self.variables]) @@ -1020,13 +1018,6 @@ def ground(self, objects: Sequence[Object]) -> _GroundNSRT: assert all( o.is_instance(p.type) for o, p in zip(objects, self.parameters)) sub = dict(zip(self.parameters, objects)) - # try: - # preconditions = {atom.ground(sub) for atom in self.preconditions} - # add_effects = {atom.ground(sub) for atom in self.add_effects} - # delete_effects = {atom.ground(sub) for atom in self.delete_effects} - # option_objs = [sub[v] for v in self.option_vars] - # except: - # import pdb; pdb.set_trace() preconditions = {atom.ground(sub) for atom in self.preconditions} add_effects = {atom.ground(sub) for atom in self.add_effects} delete_effects = {atom.ground(sub) for atom in self.delete_effects} diff --git a/predicators/utils.py b/predicators/utils.py index ef78200a5..4d012bba0 100644 --- a/predicators/utils.py +++ b/predicators/utils.py @@ -2511,14 +2511,12 @@ def get_prompt_for_vlm_state_labelling( filepath_prefix = get_path_to_predicators_root() + \ "/predicators/datasets/vlm_input_data_prompts/atom_labelling/" try: - with open(filepath_prefix + - prompt_type + ".txt", + with open(filepath_prefix + prompt_type + ".txt", "r", encoding="utf-8") as f: prompt = f.read() except FileNotFoundError: - raise ValueError("Unknown VLM prompting option " + - f"{prompt_type}") + raise ValueError("Unknown VLM prompting option " + f"{prompt_type}") # The prompt ends with a section for 'Predicates', so list these. for atom_str in atoms_list: prompt += f"\n{atom_str}" @@ -2537,10 +2535,7 @@ def get_prompt_for_vlm_state_labelling( # Now, we use actual difference-based prompting for the second timestep # and beyond. curr_prompt = prompt[:] - curr_prompt_imgs = [ - imgs_history[-2][0], - imgs_history[-1][0] - ] + curr_prompt_imgs = [imgs_history[-2][0], imgs_history[-1][0]] if CFG.vlm_include_cropped_images: if CFG.env in ["burger", "burger_no_move"]: # pragma: no cover curr_prompt_imgs.extend( @@ -2555,7 +2550,6 @@ def get_prompt_for_vlm_state_labelling( curr_prompt += "\n\nPredicate values in the first scene, " \ "before the skill was executed: \n" curr_prompt += label_history[-1] - # import pdb; pdb.set_trace() return (curr_prompt, curr_prompt_imgs) else: # NOTE: we rip out only the first image from each trajectory @@ -2620,14 +2614,14 @@ def query_vlm_for_atom_vals( # ALTERNATIVE WAY TO PARSE if len(label_history) > 0: truth_values = re.findall(r'\* (.*): (True|False)', vlm_output_str) - for i, (atom_query, pred_label) in enumerate(zip(atom_queries_list, truth_values)): + for i, (atom_query, + pred_label) in enumerate(zip(atom_queries_list, truth_values)): pred, label = pred_label assert pred in atom_query label = label.lower() if label == "true": true_atoms.add(vlm_atoms[i]) else: - # import pdb; pdb.set_trace() all_vlm_responses = vlm_output_str.strip().split("\n") # NOTE: this assumption is likely too brittle; if this is breaking, feel # free to remove/adjust this and change the below parsing loop accordingly! @@ -2641,13 +2635,13 @@ def query_vlm_for_atom_vals( value = curr_vlm_output_line.split(': ')[-1].strip('.').lower() if value == "true": true_atoms.add(vlm_atoms[i]) - + # breakpoint() # Add the text of the VLM's response to the state, to be used in the future! # REMOVE THIS -> AND PUT IT IN THE PERCEIVER - # Perceiver calls utils.abstract once, and puts it in the state history. - # According to a flag, anywhere else we normally call utils.abstract, we - # instead just pull the abstract state from the state simulator state field that has it already. + # Perceiver calls utils.abstract once, and puts it in the state history. + # According to a flag, anywhere else we normally call utils.abstract, we + # instead just pull the abstract state from the state simulator state field that has it already. # The appending of vlm atom history is currently done in query_vlm_for_atom_vals() in utils.py, # and utils.ground calls that. # state.simulator_state["vlm_atoms_history"].append(all_vlm_responses) @@ -2680,7 +2674,6 @@ def abstract(state: State, for pred in vlm_preds: for choice in get_object_combinations(list(state), pred.types): vlm_atoms.add(GroundAtom(pred, choice)) - # import pdb; pdb.set_trace() true_vlm_atoms = query_vlm_for_atom_vals(vlm_atoms, state, vlm) atoms |= true_vlm_atoms return atoms diff --git a/tests/envs/test_spot_envs.py b/tests/envs/test_spot_envs.py index f723f4f13..170d329f1 100644 --- a/tests/envs/test_spot_envs.py +++ b/tests/envs/test_spot_envs.py @@ -272,15 +272,11 @@ def real_robot_cube_env_test() -> None: "test_task_json_dir": args.get("test_task_json_dir", None), }) - import pdb - pdb.set_trace() rng = np.random.default_rng(123) import os os.environ["BOSDYN_CLIENT_USERNAME"] = "user" os.environ["BOSDYN_CLIENT_PASSWORD"] = "bbbdddaaaiii" env = SpotCubeEnv() - import pdb - pdb.set_trace() perceiver = SpotPerceiver() nsrts = get_gt_nsrts(env.get_name(), env.predicates, get_gt_options(env.get_name())) @@ -289,8 +285,6 @@ def real_robot_cube_env_test() -> None: task = env.get_test_tasks()[0] obs = env.reset("test", 0) perceiver.reset(task) - import pdb - pdb.set_trace() assert len(obs.objects_in_view) == 4 cube, floor, table1, table2 = sorted(obs.objects_in_view) assert cube.name == "cube"