some updates - making progress towards a simplified working setup

Author: NishanthJKumar

predicators/envs/spot_env.py

@@ -25,7 +25,7 @@
 from predicators.spot_utils.perception.object_detection import \
     AprilTagObjectDetectionID, KnownStaticObjectDetectionID, \
     LanguageObjectDetectionID, ObjectDetectionID, _query_detic_sam, \
-    detect_objects, visualize_all_artifacts
+    _query_vlm, detect_objects, visualize_all_artifacts
 from predicators.spot_utils.perception.object_specific_grasp_selection import \
     brush_prompt, bucket_prompt, football_prompt, train_toy_prompt
 from predicators.spot_utils.perception.perception_structs import RGBDImage, \
@@ -630,6 +630,12 @@ def step(self, action: Action) -> Observation:
                 else:
                     assert action_name == "MoveToReachObject"
                     obj_pose = self._last_known_object_poses[target_obj]
+                    logging.warning(
+                        f"MoveToReachObject using last known pose for "
+                        f"'{target_obj.name}': "
+                        f"({obj_pose.x:.3f}, {obj_pose.y:.3f}, "
+                        f"{obj_pose.z:.3f}). This pose may be STALE "
+                        f"if the object was not re-detected recently.")
                     obj_position = math_helpers.Vec3(x=obj_pose.x,
                                                      y=obj_pose.y,
                                                      z=obj_pose.z)
@@ -734,6 +740,19 @@ def _build_realworld_observation(
             if (self._detection_id_to_obj[det_id].type.name == "movable")
             # or self._detection_id_to_obj[det_id].name == "clear_plastic_container")
         }
+        for obj, pose in all_objects_in_view.items():
+            old_pose = self._last_known_object_poses.get(obj)
+            if old_pose is not None:
+                dist = np.sqrt((pose.x - old_pose.x)**2 +
+                               (pose.y - old_pose.y)**2 +
+                               (pose.z - old_pose.z)**2)
+                if dist > 0.1:
+                    logging.warning(
+                        f"Object '{obj.name}' pose changed significantly: "
+                        f"old=({old_pose.x:.3f}, {old_pose.y:.3f}, "
+                        f"{old_pose.z:.3f}) -> "
+                        f"new=({pose.x:.3f}, {pose.y:.3f}, {pose.z:.3f}) "
+                        f"(dist={dist:.3f}m)")
         self._last_known_object_poses.update(all_objects_in_view)
         objects_in_hand_view = set(self._detection_id_to_obj[det_id]
                                    for det_id in hand_detections)
@@ -1157,37 +1176,52 @@ def _actively_construct_initial_object_views(
             self) -> Tuple[Dict[Object, math_helpers.SE3Pose], Dict[str, Any]]:
         assert self._robot is not None
         assert self._localizer is not None
-        # stow_arm(self._robot)
-        # go_home(self._robot, self._localizer)
         self._localizer.localize()
-        # detection_ids = self._detection_id_to_obj.keys()
-        # import ipdb; ipdb.set_trace()
-        # detections, artifacts = self._run_init_search_for_objects(
-        #     set(detection_ids))
-        # stow_arm(self._robot)
-        # obj_to_se3_pose = {
-        #     self._detection_id_to_obj[det_id]: val
-        #     for (det_id, val) in detections.items()
-        # }
 
-        obj_to_se3_pose = get_known_movable_objects()
-        obj_to_se3_pose.update(get_known_immovable_objects())
-        # Remap object types using _detection_id_to_obj so that e.g.
-        # short_round_coffee_table gets _table_type instead of _immovable_type.
-        det_id_to_obj = self._detection_id_to_obj
-        name_to_remapped_obj = {o.name: o for o in det_id_to_obj.values()}
+        # Capture images from the current position and run detection
+        # without moving the robot.
+        rgbds = capture_images(self._robot, self._localizer)
+        detection_ids = set(self._detection_id_to_obj.keys())
+        detections, artifacts = detect_objects(
+            detection_ids, rgbds, self._allowed_regions)
+
+        if CFG.spot_render_perception_outputs:
+            outdir = Path(CFG.spot_perception_outdir)
+            time_str = time.strftime("%Y%m%d-%H%M%S")
+            detections_outfile = outdir / f"detections_{time_str}.png"
+            no_detections_outfile = outdir / f"no_detections_{time_str}.png"
+            visualize_all_artifacts(artifacts, detections_outfile,
+                                    no_detections_outfile)
+
         obj_to_se3_pose = {
-            name_to_remapped_obj.get(o.name, o): pose
-            for o, pose in obj_to_se3_pose.items()
+            self._detection_id_to_obj[det_id]: val
+            for (det_id, val) in detections.items()
         }
+
+        # For any movable objects not detected, fall back to known poses
+        # from the config map file with a warning.
+        known_movable = get_known_movable_objects()
+        det_id_to_obj = self._detection_id_to_obj
+        name_to_remapped_obj = {o.name: o for o in det_id_to_obj.values()}
+        for o, pose in known_movable.items():
+            remapped = name_to_remapped_obj.get(o.name, o)
+            if remapped not in obj_to_se3_pose:
+                logging.warning(
+                    f"Object '{o.name}' not detected by perception. "
+                    f"Falling back to known pose from config map: "
+                    f"({pose.x:.3f}, {pose.y:.3f}, {pose.z:.3f}). "
+                    f"THIS POSE MAY BE INCORRECT!")
+                obj_to_se3_pose[remapped] = pose
+
+        # Always use known poses for immovable objects.
+        known_immovable = get_known_immovable_objects()
+        for o, pose in known_immovable.items():
+            remapped = name_to_remapped_obj.get(o.name, o)
+            if remapped not in obj_to_se3_pose:
+                obj_to_se3_pose[remapped] = pose
+
         self._last_known_object_poses.update(obj_to_se3_pose)
-        # Move the robot into a good place to construct the initial state
-        # by running VLM predicates.
-        prompt = "Finished initial search for objects. Take control of the robot and move it into a good initial location for constructing the initial state of the task. Press 'Enter' when done!"
-        _ = input(prompt)
-        assert self._lease_client is not None
-        self._lease_client.take()
-        return obj_to_se3_pose, {}
+        return obj_to_se3_pose, artifacts
 
     def _run_init_search_for_objects(
         self, detection_ids: Set[ObjectDetectionID]
@@ -1235,7 +1269,7 @@ def _generate_goal_description(self) -> GoalDescription:
 ###############################################################################
 
 ## Constants
-HANDEMPTY_GRIPPER_THRESHOLD = 2.5  # made public for use in perceiver
+HANDEMPTY_GRIPPER_THRESHOLD = 4.5  # made public for use in perceiver
 _ONTOP_Z_THRESHOLD = 0.2
 _INSIDE_Z_THRESHOLD = 0.3
 _ONTOP_SURFACE_BUFFER = 0.48
@@ -2791,8 +2825,12 @@ def detect_objects(
             assert isinstance(
                 obj_id, LanguageObjectDetectionID
             ), "Only LanguageObjectDetectionIDs are supported."
-        object_id_to_img_detections = _query_detic_sam(
-            object_ids, rgbd_images)  # type: ignore
+        if CFG.spot_use_vlm_detection:
+            object_id_to_img_detections = _query_vlm(
+                object_ids, rgbd_images)  # type: ignore
+        else:
+            object_id_to_img_detections = _query_detic_sam(
+                object_ids, rgbd_images)  # type: ignore
         # 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
@@ -4417,9 +4455,9 @@ def _detection_id_to_obj(self) -> Dict[ObjectDetectionID, Object]:
             if obj.name == "short_round_coffee_table":
                 table_obj = Object("short_round_coffee_table", _table_type)
                 detection_id_to_obj[stat_detection_id] = table_obj
-            # if obj.name == "child_play_table":
-            #     table_obj = Object("child_play_table", _table_type)
-            #     detection_id_to_obj[stat_detection_id] = table_obj
+            elif obj.name == "childs_play_table":
+                table_obj = Object("childs_play_table", _table_type)
+                detection_id_to_obj[stat_detection_id] = table_obj
             else:
                 detection_id_to_obj[stat_detection_id] = obj
 

predicators/ground_truth_models/spot_env/options.py

@@ -596,49 +596,68 @@ def _move_to_reach_and_wipe_surface_policy(name: str, state: State,
                                            objects: Sequence[Object],
                                            params: Array) -> Action:
 
+    # =========================================================================
+    # WIPE IMPLEMENTATION TOGGLE
+    # Set to True  -> use iPhone-based online wiping (VLM + iPhone depth)
+    # Set to False -> use old hardcoded wiping (spot_wipe_table)
+    USE_IPHONE_WIPE = True
+    # =========================================================================
+
     def _fn() -> None:
         robot, localizer, _ = get_robot()
         target_pose = math_helpers.SE2Pose(params[0], params[1], params[2])
         navigate_to_absolute_pose(robot, localizer, target_pose)
-        # #######################
-        # # NOTE: just for testing -> ask for the eraser!
-        # # Move the hand to the side.
-        # hand_side_pose = math_helpers.SE3Pose(x=0.80,
-        #                                       y=0.0,
-        #                                       z=0.25,
-        #                                       rot=math_helpers.Quat.from_yaw(
-        #                                           -np.pi / 2))
-        # move_hand_to_relative_pose(robot, hand_side_pose)
-        # # Ask for the eraser.
-        # open_gripper(robot)
-        # # Press any key, instead of just enter. Useful for remote control.
-        # msg = "Put the brush in the robot's gripper, then press any key"
-        # utils.wait_for_any_button_press(msg)
-        # close_gripper(robot)
-        # ###################
-        # NOTE: these parameters hardcoded for a particular child_play_table
-        # object njk is experimenting with. Please swap out depending on the
-        # actual object you have
-        # start_pose = math_helpers.SE3Pose(x=0.8,
-        #                                   y=-0.35,
-        #                                   z=-0.08,
-        #                                   rot=math_helpers.Quat.from_pitch(
-        #                                       np.pi / 2))
-        start_pose = math_helpers.SE3Pose(x=0.8,
-                                          y=-0.1,
-                                          z=-0.1,
-                                          rot=math_helpers.Quat.from_pitch(
-                                              np.pi / 2))
-        end_pose = math_helpers.SE3Pose(x=0.65,
-                                        y=0.0,
-                                        z=0.55,
-                                        rot=math_helpers.Quat.from_pitch(
-                                            np.pi / 2))
-        rel_dx, rel_dy, delta_dx, delta_dy, num_wipes, duration_per_stroke = params[
-            3:9]
-        wipe_multiple_strokes(robot, start_pose, end_pose,
-                              rel_dx, rel_dy, (delta_dx, delta_dy),
-                              int(num_wipes), duration_per_stroke, int(2))
+
+        if USE_IPHONE_WIPE:
+            # --- NEW: iPhone-based online wiping ---
+            # Uses VLM (Gemini) to detect the spill via iPhone depth camera,
+            # then computes wipe params from the bounding box automatically.
+            # Requires iPhone streaming to be running.
+            # Lazy import to avoid pulling in open3d/rerun/etc at module load.
+            from predicators.spot_utils.skills.spot_wipe_online_iphone import \
+                wipe_online
+            wipe_online(robot)
+        else:
+            # --- OLD: Hardcoded wiping (spot_wipe_table) ---
+            # #######################
+            # # NOTE: just for testing -> ask for the eraser!
+            # # Move the hand to the side.
+            # hand_side_pose = math_helpers.SE3Pose(x=0.80,
+            #                                       y=0.0,
+            #                                       z=0.25,
+            #                                       rot=math_helpers.Quat.from_yaw(
+            #                                           -np.pi / 2))
+            # move_hand_to_relative_pose(robot, hand_side_pose)
+            # # Ask for the eraser.
+            # open_gripper(robot)
+            # # Press any key, instead of just enter. Useful for remote control.
+            # msg = "Put the brush in the robot's gripper, then press any key"
+            # utils.wait_for_any_button_press(msg)
+            # close_gripper(robot)
+            # ###################
+            # NOTE: these parameters hardcoded for a particular child_play_table
+            # object njk is experimenting with. Please swap out depending on the
+            # actual object you have
+            # start_pose = math_helpers.SE3Pose(x=0.8,
+            #                                   y=-0.35,
+            #                                   z=-0.08,
+            #                                   rot=math_helpers.Quat.from_pitch(
+            #                                       np.pi / 2))
+            start_pose = math_helpers.SE3Pose(x=0.8,
+                                              y=-0.1,
+                                              z=-0.1,
+                                              rot=math_helpers.Quat.from_pitch(
+                                                  np.pi / 2))
+            end_pose = math_helpers.SE3Pose(x=0.65,
+                                            y=0.0,
+                                            z=0.55,
+                                            rot=math_helpers.Quat.from_pitch(
+                                                np.pi / 2))
+            rel_dx, rel_dy, delta_dx, delta_dy, num_wipes, \
+                duration_per_stroke = params[3:9]
+            wipe_multiple_strokes(robot, start_pose, end_pose,
+                                  rel_dx, rel_dy, (delta_dx, delta_dy),
+                                  int(num_wipes), duration_per_stroke, int(2))
 
     # Note simulation fn and args not implemented yet.
     action_extra_info = SpotActionExtraInfo(name, objects, _fn, tuple(), None,

predicators/perception/spot_perceiver.py

@@ -697,8 +697,8 @@ def _create_goal(self, state: State,
             clear_trash_can = Object("clear_plastic_container", _trash_can_type)
             cardboard_trash_can = Object("cardboard_box_bin", _trash_can_type)
             apple = Object("apple", _movable_object_type)
-            table = Object("short_round_coffee_table", _table_type)
             # table = Object("short_round_coffee_table", _table_type)
+            table = Object("childs_play_table", _table_type)
             eraser = Object("pink_furry_eraser", _movable_object_type)
             recycling_bin = Object("recycling_bin", _trash_can_type)
             soda = Object("soda_can", _movable_object_type)

predicators/pretrained_model_interface.py

@@ -310,7 +310,7 @@ def _sample_completions(
         response = self._client.models.generate_content(
             model=self._model_name,
             contents=[prompt] + imgs,
-            config=config)
+            config=config)    
         return [response.text]
 
     def get_id(self) -> str:

predicators/settings.py

@@ -179,6 +179,8 @@ class GlobalSettings:
     spot_robot_ip = "invalid-IP-address"
     spot_fiducial_size = 44.45
     spot_vision_detection_threshold = 0.5
+    spot_use_vlm_detection = True
+    spot_vlm_detection_default_score = 0.9
     spot_perception_outdir = "spot_perception_outputs"
     spot_render_perception_outputs = True
     spot_graph_nav_map = "floor8-sweeping"

predicators/spot_utils/graph_nav_maps/b45-621-cleanup/metadata.yaml

@@ -31,7 +31,11 @@ known-immovable-objects:
     x: 1.4
     y: 0.5
     z: -0.5
-  short_round_coffee_table:
+  # short_round_coffee_table:
+  #   x: 1.65
+  #   y: -0.24
+  #   z: 0.14
+  childs_play_table:
     x: 1.65
     y: -0.24
     z: 0.14
@@ -42,7 +46,12 @@ known-movable-objects:
   #   y: 0.3
   #   z: 0.20
   #   object_type: "movable"
-  pink_furry_eraser:
+  # pink_furry_eraser:
+  #   x: 2.5
+  #   y: -0.5
+  #   z: 0.20
+  #   object_type: "movable"
+  green_and_blue_furry_eraser:
     x: 2.5
     y: -0.5
     z: 0.20
@@ -130,6 +139,13 @@ static-object-features:
     width: 0.1
     placeable: 1
     is_sweeper: 0
+  green_and_blue_furry_eraser:
+    shape: 1
+    height: 0.1
+    length: 0.1
+    width: 0.1
+    placeable: 1
+    is_sweeper: 0
   soda_can:
     shape: 2
     height: 0.1
@@ -151,15 +167,24 @@ static-object-features:
     length: 0.79
     width: 0.62
     flat_top_surface: 1
+  childs_play_table:
+    shape: 2
+    height: 0.1
+    length: 0.35
+    width: 0.62
+    flat_top_surface: 1
 
 # Helpful for static objects that are up against a wall, for example.
 approach_angle_bounds:
   short_round_coffee_table: [-0.1, 0.1]
+  childs_play_table: [-0.1, 0.1]
   clear_plastic_container: [1.47, 1.67]
   recycling_bin: [-0.1, 0.1]
   pink_furry_eraser: [-1.67, -1.47]
+  green_and_blue_furry_eraser: [-1.67, -1.47]
   soda_can: [-1.67, -1.47]
 
 # Wiping skill params.
 wipe_location:
-  short_round_coffee_table: [2.48, 0.205, -2.96856]
+  short_round_coffee_table: [2.48, 0.205, -2.96856]
+  childs_play_table: [2.48, 0.205, -2.96856]