Tune coffee skills: fix button press, gradual fill, table collision, pour

- Fix button press to use Euclidean distance instead of squared distance
- Increase button press threshold for pybullet coffee
- Stop re-creating liquid in gradual fill once jug is full
- Add table to collision bodies in motion planning
- Skip MoveAbovePour phase in pour skill
- Reduce liquid body mass

[coffee-new-skills]

Author: yichao-liang

predicators/envs/coffee.py

@@ -795,8 +795,8 @@ def _PressingButton_holds(self, state: State,
         x = state.get(robot, "x")
         y = state.get(robot, "y")
         z = state.get(robot, "z")
-        sq_dist_to_button = np.sum(np.subtract(button_pos, (x, y, z))**2)
-        return sq_dist_to_button < self.button_press_threshold
+        dist_to_button = np.sqrt(np.sum(np.subtract(button_pos, (x, y, z))**2))
+        return dist_to_button < self.button_press_threshold
 
     @staticmethod
     def _NotSameCup_holds(state: State, objects: Sequence[Object]) -> bool:

predicators/envs/pybullet_coffee.py

@@ -113,7 +113,7 @@ class PyBulletCoffeeEnv(PyBulletEnv, CoffeeEnv):
     button_y: ClassVar[float] =\
         machine_y - machine_y_len / 2 - machine_top_y_len - button_height/2
     button_z: ClassVar[float] = z_lb + machine_z_len - button_radius
-    button_press_threshold: ClassVar[float] = 1e-2
+    button_press_threshold: ClassVar[float] = 3e-2
     machine_color: ClassVar[Tuple[float, float, float, float]] =\
         (0.1, 0.1, 0.1, 1) # Black
     button_color_on: ClassVar[Tuple[float, float, float,
@@ -575,6 +575,18 @@ def _handle_machine_on_and_jug_filling(self, state: State) -> None:
                          or self._machine_plugged_in_id is not None)):
                 if CFG.coffee_fill_jug_gradually:
                     # Gradual filling
+                    if self._jug_current_liquid < self.max_jug_coffee_capacity:
+                        self._jug_current_liquid = min(
+                            self.max_jug_coffee_capacity,
+                            self._jug_current_liquid +
+                            self.coffee_machine_fill_speed)
+                        self._jug_liquid_id = self._create_liquid_for_jug()
+                        if (not self._jug_filled and
+                                self._jug_current_liquid >
+                                self.coffee_filled_threshold):
+                            self._jug_filled = True
+                else:
+                    # Instant filling
                     self._jug_current_liquid = min(
                         self.max_jug_coffee_capacity,
                         self._jug_current_liquid +
@@ -583,11 +595,6 @@ def _handle_machine_on_and_jug_filling(self, state: State) -> None:
                     if (not self._jug_filled and self._jug_current_liquid >
                             self.coffee_filled_threshold):
                         self._jug_filled = True
-                else:
-                    # Instant filling
-                    if not self._jug_filled:
-                        self._jug_liquid_id = self._create_liquid_for_jug()
-                    self._jug_filled = True
 
     def _handle_pouring(self, state: State) -> None:
         """If the robot is tilted sufficiently to pour, increase liquid in the
@@ -781,7 +788,7 @@ def _create_liquid_for_jug(self) -> int:
         liquid_pos = (jug_pos[0], jug_pos[1],
                       self.z_lb + liquid_height / 2 + 0.02)
 
-        return p.createMultiBody(baseMass=0.001,
+        return p.createMultiBody(baseMass=1e-5,
                                  baseCollisionShapeIndex=collision_id,
                                  baseVisualShapeIndex=visual_id,
                                  basePosition=liquid_pos,

predicators/ground_truth_models/skill_factories/base.py

@@ -544,7 +544,11 @@ def _plan_with_simulator(
                 continue
             collision_bodies.add(sim_obj.id)
 
-        # 4b. Add extra sim collision bodies (e.g. virtual buffer zones).
+        # 4b. Add the table if present.
+        if hasattr(sim, '_table') and sim._table.id is not None:
+            collision_bodies.add(sim._table.id)
+
+        # 4c. Add extra sim collision bodies (e.g. virtual buffer zones).
         collision_bodies.update(self._config.sim_extra_collision_bodies)
 
         # 5. IK + motion planning on simulator's robot

predicators/ground_truth_models/skill_factories/pour.py

@@ -124,8 +124,8 @@ def _tilt_target(
         return current_pose, target_pose, "closed"
 
     phases = [
-        # Phase 0: Move above pour position at normal tilt
-        make_move_to_phase("MoveAbovePour", _above_pose, "closed"),
+        # # Phase 0: Move above pour position at normal tilt
+        # make_move_to_phase("MoveAbovePour", _above_pose, "closed"),
         # Phase 1: Descend to pour height at normal tilt
         make_move_to_phase("DescendToPour", _descend_pose, "closed"),
         # Phase 2: Tilt EE to pour angle (incremental IK for fine control)