From 950ea7ae7252421fd781718ccbbdb4f37384a9e7 Mon Sep 17 00:00:00 2001 From: Yichao Liang Date: Sun, 16 Aug 2026 13:58:49 -0400 Subject: [PATCH 01/30] motion planning: collision-check welded partners of the held object The BiRRT helper modeled a single held body; bodies weld-attached to it were dropped from the collision set entirely ("welded partners sweep unchecked"), so transporting the bridge span row swept its outer spans through a standing leg and toppled it in 3/3 oracle bridge runs. Pose every welded partner rigidly with the arm and collision-check it exactly like the held object itself (run_motion_planning gains a held_attachments map of end-effector-relative transforms, captured by the skill factory from the planning simulator). The collision diagnostics and the stall report cover the attachments too, and a new regression test rejects a goal that sweeps an attachment into an obstacle. --- .../skill_factories/base.py | 104 +++++++++++++----- .../pybullet_helpers/motion_planning.py | 82 +++++++++----- .../pybullet_helpers/test_motion_planning.py | 98 +++++++++++++++++ 3 files changed, 225 insertions(+), 59 deletions(-) diff --git a/predicators/ground_truth_models/skill_factories/base.py b/predicators/ground_truth_models/skill_factories/base.py index ef6ebe748..6cd6e6ca6 100644 --- a/predicators/ground_truth_models/skill_factories/base.py +++ b/predicators/ground_truth_models/skill_factories/base.py @@ -24,6 +24,7 @@ from predicators.pybullet_helpers.inverse_kinematics import \ InverseKinematicsError from predicators.pybullet_helpers.joint import JointPositions +from predicators.pybullet_helpers.link import get_link_state from predicators.pybullet_helpers.motion_planning import run_motion_planning from predicators.pybullet_helpers.robots.single_arm import \ SingleArmPyBulletRobot @@ -928,13 +929,17 @@ def _plan_without_simulator( def _sim_collision_context( self, pb_state: utils.PyBulletState - ) -> Tuple[utils.PyBulletState, set, Dict[int, str], Optional[int]]: + ) -> Tuple[utils.PyBulletState, set, Dict[int, str], Optional[int], Dict[ + int, Any]]: """Remap ``pb_state`` onto the planning simulator and collect its collision bodies. Resets the simulator to the remapped state as a side effect. Returns ``(remapped_state, collision_bodies, body_names, - held_object)``. Requires ``self._config.simulator``. + held_object, held_attachments)``, where ``held_attachments`` + maps each body weld-attached to the held object to its end- + effector-relative transform (see ``run_motion_planning``). + Requires ``self._config.simulator``. """ sim = self._config.simulator assert sim is not None @@ -974,15 +979,36 @@ def _sim_collision_context( continue collision_bodies.add(sim_obj.id) - # 4a. Exclude bodies weld-attached to the held object (e.g. a glued - # assembly transported as a rigid unit, see pybullet_bridge). - # They travel with the grasped body, so treating them as static - # obstacles would make every transport plan collide immediately. - # Conservative approximation: welded partners sweep unchecked. + # 4a. Bodies weld-attached to the held object (e.g. a glued assembly + # transported as a rigid unit, see pybullet_bridge) travel with + # the grasped body, so treating them as static obstacles would + # make every transport plan collide immediately. Remove them from + # the obstacle set and instead hand them to the motion planner as + # rigid attachments of the held object (posed with the arm and + # collision-checked like the held object itself), capturing their + # end-effector-relative transforms from the just-reset simulator. + held_attachments: Dict[int, Any] = {} if held_object is not None: get_welded = getattr(sim, "get_welded_partner_ids", None) if get_welded is not None: - collision_bodies -= set(get_welded(held_object)) + welded_ids = set(get_welded(held_object)) + collision_bodies -= welded_ids + if welded_ids: + client = sim._physics_client_id # pylint: disable=protected-access + planning_robot = sim._pybullet_robot # pylint: disable=protected-access + planning_robot.set_joints(pb_state.joint_positions) + world_to_base_link = get_link_state( + planning_robot.robot_id, + planning_robot.end_effector_id, + physics_client_id=client).com_pose + base_link_to_world = p.invertTransform( + world_to_base_link[0], world_to_base_link[1]) + for welded_id in welded_ids: + world_to_obj = p.getBasePositionAndOrientation( + welded_id, physicsClientId=client) + held_attachments[welded_id] = p.multiplyTransforms( + base_link_to_world[0], base_link_to_world[1], + world_to_obj[0], world_to_obj[1]) # 4b. Add tables if present. if hasattr(sim, '_table_ids'): @@ -998,7 +1024,8 @@ def _sim_collision_context( # blocks in Grow that aren't tracked as state Objects). collision_bodies.update(sim.get_extra_collision_ids()) - return remapped_state, collision_bodies, body_names, held_object + return remapped_state, collision_bodies, body_names, held_object, \ + held_attachments def _stall_contact_report(self, pb_state: utils.PyBulletState) -> str: """Name the bodies the robot is touching when incremental IK stalls. @@ -1014,7 +1041,7 @@ def _stall_contact_report(self, pb_state: utils.PyBulletState) -> str: return "" try: sim = self._config.simulator - _, collision_bodies, body_names, held_object = \ + _, collision_bodies, body_names, held_object, held_attachments = \ self._sim_collision_context(pb_state) planning_robot = sim._pybullet_robot # pylint: disable=protected-access planning_robot.set_joints(pb_state.joint_positions) @@ -1028,11 +1055,16 @@ def _stall_contact_report(self, pb_state: utils.PyBulletState) -> str: # needs the blocker named. margin = max(CFG.pybullet_birrt_contact_margin, CFG.pybullet_birrt_bystander_clearance) + probes = [(planning_robot.robot_id, "robot"), + (held_object, "held object")] + probes.extend( + (attached_id, + f"welded {body_names.get(attached_id, attached_id)}") + for attached_id in held_attachments) touching = [] for body in sorted(collision_bodies): label = body_names.get(body, f"body {body}") - for probe, probe_label in ((planning_robot.robot_id, "robot"), - (held_object, "held object")): + for probe, probe_label in probes: if probe is None: continue contacts = p.getContactPoints(probe, @@ -1066,8 +1098,8 @@ def _plan_with_simulator( del objects # Unused; kept for a uniform planner signature. sim = self._config.simulator assert sim is not None - remapped_state, collision_bodies, body_names, held_object = \ - self._sim_collision_context(pb_state) + remapped_state, collision_bodies, body_names, held_object, \ + held_attachments = self._sim_collision_context(pb_state) # 5. IK + motion planning on simulator's robot planning_robot = sim._pybullet_robot # pylint: disable=protected-access @@ -1118,6 +1150,7 @@ def _plan_with_simulator( physics_client_id=sim._physics_client_id, # pylint: disable=protected-access held_object=held_object, base_link_to_held_obj=base_link_to_held_obj, + held_attachments=held_attachments, allow_shallow_held_object_contacts=( phase.allow_shallow_held_object_contacts if phase is not None else False), @@ -1153,6 +1186,7 @@ def _plan_with_simulator( physics_client_id=sim._physics_client_id, # pylint: disable=protected-access held_object=held_object, base_link_to_held_obj=base_link_to_held_obj, + held_attachments=held_attachments, allow_shallow_held_object_contacts=( phase.allow_shallow_held_object_contacts if phase is not None else False), @@ -1174,7 +1208,8 @@ def _plan_with_simulator( base_link_to_held_obj, phase_name, body_names=body_names, - goal_finger_joint=goal_finger_joint) + goal_finger_joint=goal_finger_joint, + held_attachments=held_attachments) return traj @@ -1256,6 +1291,7 @@ def _log_collision_diagnostics( phase_name: str, body_names: Optional[Dict[int, str]] = None, goal_finger_joint: Optional[float] = None, + held_attachments: Optional[Dict[int, Any]] = None, ) -> List[str]: """Log which collision bodies cause start/goal collisions. @@ -1264,8 +1300,6 @@ def _log_collision_diagnostics( is the only channel through which it learns WHICH object blocked the motion plan (and hence how to adjust its target pose). """ - from predicators.pybullet_helpers.link import \ - get_link_state # pylint: disable=import-outside-toplevel diagnostics: List[str] = [] def _body_label(body: int) -> str: @@ -1279,21 +1313,31 @@ def _body_label(body: int) -> str: pass return f"body {body} ({body_name})" + held_assembly: List[Tuple[int, Any, str]] = [] + if held_object is not None and base_link_to_held_obj is not None: + held_assembly.append( + (held_object, base_link_to_held_obj, "held object")) + held_assembly.extend( + (attached_id, transform, + f"welded {(body_names or {}).get(attached_id, attached_id)}") + for attached_id, transform in (held_attachments or {}).items()) + def _check(joints: JointPositions, label: str) -> None: planning_robot.set_joints(joints) - if held_object is not None and base_link_to_held_obj is not None: + if held_assembly: wt_bl = get_link_state( planning_robot.robot_id, planning_robot.end_effector_id, physics_client_id=physics_client_id).com_pose - wt_ho = p.multiplyTransforms(wt_bl[0], wt_bl[1], - base_link_to_held_obj[0], - base_link_to_held_obj[1]) - p.resetBasePositionAndOrientation( - held_object, - wt_ho[0], - wt_ho[1], - physicsClientId=physics_client_id) + for assembly_body, base_link_to_obj, _ in held_assembly: + wt_obj = p.multiplyTransforms(wt_bl[0], wt_bl[1], + base_link_to_obj[0], + base_link_to_obj[1]) + p.resetBasePositionAndOrientation( + assembly_body, + wt_obj[0], + wt_obj[1], + physicsClientId=physics_client_id) p.performCollisionDetection(physicsClientId=physics_client_id) # Report against the wider of the two thresholds so that # bystander-clearance failures (positive separations) are @@ -1310,14 +1354,14 @@ def _check(joints: JointPositions, label: str) -> None: diagnostics.append( f"{label}: robot within {min_dist:.4f} m of " f"{_body_label(body)}") - if held_object is not None: + for assembly_body, _, assembly_label in held_assembly: contacts = p.getContactPoints( - held_object, body, physicsClientId=physics_client_id) + assembly_body, body, physicsClientId=physics_client_id) if any(c[8] < margin for c in contacts): min_dist = min(c[8] for c in contacts) diagnostics.append( - f"{label}: held object within {min_dist:.4f} m " - f"of {_body_label(body)}") + f"{label}: {assembly_label} within " + f"{min_dist:.4f} m of {_body_label(body)}") _check(start_joints, "START") _check(goal_joints, "GOAL") diff --git a/predicators/pybullet_helpers/motion_planning.py b/predicators/pybullet_helpers/motion_planning.py index 7fa739e7b..fb7a8caa1 100644 --- a/predicators/pybullet_helpers/motion_planning.py +++ b/predicators/pybullet_helpers/motion_planning.py @@ -1,7 +1,8 @@ """Motion Planning in PyBullet.""" from __future__ import annotations -from typing import Collection, Iterator, Optional, Sequence +from typing import Any, Collection, Dict, Iterator, List, Optional, Sequence, \ + Tuple import numpy as np import pybullet as p @@ -24,6 +25,7 @@ def run_motion_planning( physics_client_id: int, held_object: Optional[int] = None, base_link_to_held_obj: Optional[NDArray] = None, + held_attachments: Optional[Dict[int, Any]] = None, allow_shallow_held_object_contacts: bool = False, goal_finger_joint: Optional[float] = None, held_bystander_clearance: Optional[float] = None, @@ -36,6 +38,14 @@ def run_motion_planning( bystanders from which the path must keep ``CFG.pybullet_birrt_bystander_clearance`` of separation. + ``held_attachments`` maps bodies rigidly attached to the held object + (e.g. the welded members of a glued assembly) to their + base-link-relative transforms, in the same ``(position, orientation)`` + convention as ``base_link_to_held_obj``. They move with the arm and + are collision-checked exactly like the held object itself, so a + transported assembly cannot sweep an unchecked member through a + bystander. They must not appear in ``collision_bodies``. + When ``goal_finger_joint`` is given, the goal configuration is additionally checked with both finger joints at that value (e.g. a place phase whose next phase opens the gripper: the opening sweep is @@ -69,37 +79,47 @@ def _sample_fn(pt: JointPositions) -> JointPositions: new_pt[robot.right_finger_joint_idx] = pt[robot.right_finger_joint_idx] return new_pt + # The held object and every body rigidly attached to it move with + # the arm; collision-wise they form one assembly. + held_assembly: List[Tuple[int, Any]] = [] + if held_object is not None: + assert base_link_to_held_obj is not None + held_assembly.append((held_object, base_link_to_held_obj)) + held_assembly.extend((held_attachments or {}).items()) + def _set_state(pt: JointPositions) -> None: robot.set_joints(pt) - if held_object is not None: - assert base_link_to_held_obj is not None + if held_assembly: world_to_base_link = get_link_state( robot.robot_id, robot.end_effector_id, physics_client_id=physics_client_id).com_pose - world_to_held_obj = p.multiplyTransforms(world_to_base_link[0], - world_to_base_link[1], - base_link_to_held_obj[0], - base_link_to_held_obj[1]) - p.resetBasePositionAndOrientation( - held_object, - world_to_held_obj[0], - world_to_held_obj[1], - physicsClientId=physics_client_id) + for body, base_link_to_obj in held_assembly: + world_to_obj = p.multiplyTransforms(world_to_base_link[0], + world_to_base_link[1], + base_link_to_obj[0], + base_link_to_obj[1]) + p.resetBasePositionAndOrientation( + body, + world_to_obj[0], + world_to_obj[1], + physicsClientId=physics_client_id) hard_margin = CFG.pybullet_birrt_contact_margin shallow_margin = CFG.pybullet_birrt_shallow_held_contact_margin bystander_clearance = CFG.pybullet_birrt_bystander_clearance allowed_shallow_held_collision_bodies = set() - if allow_shallow_held_object_contacts and held_object is not None: + if allow_shallow_held_object_contacts and held_assembly: _set_state(initial_positions) p.performCollisionDetection(physicsClientId=physics_client_id) for body in collision_bodies: - contacts = p.getContactPoints(held_object, - body, - physicsClientId=physics_client_id) - penetrating = [c[8] for c in contacts if c[8] < hard_margin] + penetrating: List[float] = [] + for assembly_body, _ in held_assembly: + contacts = p.getContactPoints( + assembly_body, body, physicsClientId=physics_client_id) + penetrating.extend(c[8] for c in contacts + if c[8] < hard_margin) if penetrating and min(penetrating) >= shallow_margin: allowed_shallow_held_collision_bodies.add(body) @@ -139,18 +159,21 @@ def _set_state(pt: JointPositions) -> None: bystander_clearance, physicsClientId=physics_client_id): contact_partners.add(body) - if held_object is None or body in held_near_endpoint: + if not held_assembly or body in held_near_endpoint: continue - held_pts = p.getClosestPoints( - held_object, - body, - held_probe_radius, - physicsClientId=physics_client_id) - if held_pts: - if min(pt[8] for pt in held_pts) < bystander_clearance: + held_dists: List[float] = [] + for assembly_body, _ in held_assembly: + held_pts = p.getClosestPoints( + assembly_body, + body, + held_probe_radius, + physicsClientId=physics_client_id) + held_dists.extend(held_pt[8] for held_pt in held_pts) + if held_dists: + if min(held_dists) < bystander_clearance: contact_partners.add(body) held_near_endpoint.add(body) - if held_object is not None and held_clearance > bystander_clearance: + if held_assembly and held_clearance > bystander_clearance: held_body_clearances = { body: held_clearance for body in collision_bodies if body not in held_near_endpoint @@ -187,7 +210,7 @@ def _collision_fn(pt: JointPositions) -> bool: physicsClientId=physics_client_id) if any(c[8] < margin for c in contacts): return True - if held_object is not None: + for assembly_body, _ in held_assembly: # Clearances above Bullet's contactBreakingThreshold # (0.02 m) would be silently unenforced: getContactPoints # generates no points beyond it. Query closest points out @@ -195,12 +218,13 @@ def _collision_fn(pt: JointPositions) -> bool: # applies. held_margin = held_body_clearances.get(body, margin) contacts = p.getClosestPoints( - held_object, + assembly_body, body, held_margin, physicsClientId=physics_client_id) \ if held_margin > 0 else p.getContactPoints( - held_object, body, physicsClientId=physics_client_id) + assembly_body, body, + physicsClientId=physics_client_id) contact_distances = [c[8] for c in contacts] if body in allowed_shallow_held_collision_bodies: if any(d < shallow_margin for d in contact_distances): diff --git a/tests/pybullet_helpers/test_motion_planning.py b/tests/pybullet_helpers/test_motion_planning.py index 529bcf654..1620f3f16 100644 --- a/tests/pybullet_helpers/test_motion_planning.py +++ b/tests/pybullet_helpers/test_motion_planning.py @@ -188,6 +188,104 @@ def test_bystander_clearance(physics_client_id): p.removeBody(block_id, physicsClientId=physics_client_id) +def test_held_attachments(physics_client_id): + """Bodies rigidly attached to the held object are collision-checked. + + A goal that keeps the held object itself clear of an obstacle but + sweeps a welded attachment into it must be rejected; the same goal + without the attachment plans fine. + """ + utils.reset_config({}) + ee_home_position = (1.35, 0.75, 0.75) + ee_orn = p.getQuaternionFromEuler([0.0, np.pi / 2, -np.pi]) + ee_home_pose = Pose(ee_home_position, ee_orn) + robot = create_single_arm_pybullet_robot("fetch", physics_client_id, + ee_home_pose) + robot_init_state = tuple(ee_home_position) + tuple( + ee_orn, ) + (robot.open_fingers, ) + robot.reset_state(robot_init_state) + joint_initial = robot.get_joints() + block_kwargs = { + "color": (0.0, 0.0, 1.0, 1.0), + "half_extents": (0.03, 0.03, 0.03), + # Nonzero mass: Bullet generates no contacts between two static + # bodies, and the obstacle below is static. + "mass": 0.1, + "friction": 1, + "orientation": [0., 0., 0., 1.], + "physics_client_id": physics_client_id, + } + # The held object hangs 10 cm under the end effector; a welded + # partner sits 15 cm to its +y side (like a row member). + held_id = create_pybullet_block(**block_kwargs) + held_position = np.add(ee_home_position, (0.0, 0.0, -0.1)) + p.resetBasePositionAndOrientation(held_id, + held_position, [0., 0., 0., 1.], + physicsClientId=physics_client_id) + attached_id = create_pybullet_block(**block_kwargs) + attached_position = np.add(held_position, (0.0, 0.15, 0.0)) + p.resetBasePositionAndOrientation(attached_id, + attached_position, [0., 0., 0., 1.], + physicsClientId=physics_client_id) + world_to_base_link = get_link_state( + robot.robot_id, + robot.end_effector_id, + physics_client_id=physics_client_id).com_pose + base_link_to_world = p.invertTransform(world_to_base_link[0], + world_to_base_link[1]) + base_link_to_held = p.multiplyTransforms(base_link_to_world[0], + base_link_to_world[1], + held_position, [0., 0., 0., 1.]) + base_link_to_attached = p.multiplyTransforms(base_link_to_world[0], + base_link_to_world[1], + attached_position, + [0., 0., 0., 1.]) + # Static obstacle exactly where the ATTACHED body ends up after the + # planned 10 cm descent; the held object and the robot stay clear. + obstacle_id = create_pybullet_block(color=(1.0, 0.0, 0.0, 1.0), + half_extents=(0.05, 0.05, 0.05), + mass=0, + friction=1, + orientation=[0., 0., 0., 1.], + physics_client_id=physics_client_id) + p.resetBasePositionAndOrientation(obstacle_id, + np.add(attached_position, + (0.0, 0.0, -0.1)), + [0., 0., 0., 1.], + physicsClientId=physics_client_id) + ee_target = Pose(tuple(np.add(ee_home_position, (0.0, 0.0, -0.1))), ee_orn) + joint_target = robot.inverse_kinematics(ee_target, validate=True) + # With the attachment checked, the goal sweeps it into the obstacle. + path = run_motion_planning( + robot, + joint_initial, + joint_target, + collision_bodies={obstacle_id}, + seed=123, + physics_client_id=physics_client_id, + held_object=held_id, + base_link_to_held_obj=base_link_to_held, + held_attachments={attached_id: base_link_to_attached}) + assert path is None + # Without the attachment, the same goal plans fine. + path = None + for seed in [123, 456, 789]: + robot.set_joints(joint_initial) + path = run_motion_planning(robot, + joint_initial, + joint_target, + collision_bodies={obstacle_id}, + seed=seed, + physics_client_id=physics_client_id, + held_object=held_id, + base_link_to_held_obj=base_link_to_held) + if path is not None: + break + assert path is not None + for body in (held_id, attached_id, obstacle_id): + p.removeBody(body, physicsClientId=physics_client_id) + + def test_move_to_shelf(): """Test for Panda robot moving to put a held block into a shelf. From f97e838d846ecdc723c659caad46469d865b432a Mon Sep 17 00:00:00 2001 From: Yichao Liang Date: Sun, 16 Aug 2026 13:58:59 -0400 Subject: [PATCH 02/30] bridge: harden oracle place samplers and glue-reach MoveTo bounds Three E2E failure modes seen in the oracle process-planning runs: - Halve the leg release drop (8-13 mm -> 4-6 mm): the 2:1 leg could land rocking near its tipping balance and slowly topple ~30 steps after release with nothing touching it. - PlaceSpanNextTo: two-sided +/-1 mm jitter instead of one-sided +0-4 mm, and a softer drop. Params are frozen at planning time, so the left neighbor's own landing error stacks on any outward bias against the ~1 cm cure-window margin. - SeatSpan3: center the row by the midpoint of the outer spans' actual centers instead of the grasped middle span, so weld-frozen placement offsets split across both seat joints instead of landing on one. - Extend MoveTo's x params bounds one span half-length past the block workspace: a block staged at the workspace edge has its end-face dab point up to 5 cm outside it, and the params clamp silently parked the glue tip 2.5 cm short of the dab (outside the 2 cm wetting radius), so the face never wet and the joint never cured. A genuinely unreachable dab now fails IK loudly and triggers a replan instead. With these plus the welded-partner motion-planning fix, the oracle arm solves the bridge task 3/3 on seeds 0-2. --- .../ground_truth_models/bridge/options.py | 14 +++++- .../ground_truth_models/bridge/processes.py | 46 +++++++++++++------ 2 files changed, 44 insertions(+), 16 deletions(-) diff --git a/predicators/ground_truth_models/bridge/options.py b/predicators/ground_truth_models/bridge/options.py index d0397d891..1525b4ee7 100644 --- a/predicators/ground_truth_models/bridge/options.py +++ b/predicators/ground_truth_models/bridge/options.py @@ -47,10 +47,20 @@ # object goes regardless of grasp depth or the pick's IK residual. The # glue samplers use this to land the held bottle's tip on a face dab # point (tip = center minus the bottle half-height). +# +# The x bounds extend one span half-length past the block workspace: a +# block STAGED near the workspace edge has its end-face dab point up to +# span_half_x outside it, and clamping the glue target to the block +# workspace silently parked the bottle tip 2.5 cm short of the dab -- +# outside the 2 cm wetting radius, so the face never wet and the joint +# never cured. With the wider box a genuinely unreachable dab fails IK +# loudly (and triggers a replan) instead of "succeeding" without glue. _BRIDGE_MOVE_TO_PARAMS = [ ("target_x (world x position for the held object, or the EE if " - "empty-handed)", PyBulletBridgeEnv.workspace_x_lo, - PyBulletBridgeEnv.workspace_x_hi), + "empty-handed)", + PyBulletBridgeEnv.workspace_x_lo - PyBulletBridgeEnv.span_half_extents[0], + PyBulletBridgeEnv.workspace_x_hi + + PyBulletBridgeEnv.span_half_extents[0]), ("target_y (world y position for the held object, or the EE if " "empty-handed)", 1.1, 1.6), ("target_z (world z height for the held object, or the EE if " diff --git a/predicators/ground_truth_models/bridge/processes.py b/predicators/ground_truth_models/bridge/processes.py index 44ccab562..150f1dc20 100644 --- a/predicators/ground_truth_models/bridge/processes.py +++ b/predicators/ground_truth_models/bridge/processes.py @@ -40,8 +40,6 @@ # joints freeze landing error into the weld; 8 mm still clears the # BiRRT contact margin with room for mm-level execution error. _DROP = 0.008 -_LEG_CENTER = _TABLE + _ENV.leg_half_extents[2] + _DROP # on table: 0.458 -_SPAN_CENTER = _TABLE + _ENV.span_half_extents[2] + _DROP # on table: 0.433 def _pick_sampler(state: State, goal: Set[GroundAtom], @@ -92,7 +90,12 @@ def _place_leg_at_site_sampler(state: State, goal: Set[GroundAtom], site = objs[2] x = state.get(site, "x") + rng.uniform(-0.003, 0.003) y = state.get(site, "y") + rng.uniform(-0.003, 0.003) - z = _LEG_CENTER + rng.uniform(0.0, 0.005) + # Half the generic drop: a standing leg is a 2:1 block, and an + # 8-13 mm drop can land it rocking near its tipping balance -- one + # observed leg leaned ~0.6 deg at release and slowly toppled ~30 + # steps later with nothing touching it. 4-6 mm sheds most of the + # landing energy while still clearing the BiRRT contact margin. + z = _TABLE + _ENV.leg_half_extents[2] + 0.004 + rng.uniform(0.0, 0.002) return np.array([x, y, z, 0.0], dtype=np.float32) @@ -102,15 +105,21 @@ def _place_next_to_sampler(state: State, goal: Set[GroundAtom], del goal # objs = [robot, right, left]: butt the held block against the left # block's end_b (+x) face, with a small nominal gap so the landing - # does not shove the (wet-glued) left block out of alignment. + # does not shove the (wet-glued) left block out of alignment. Keep + # the jitter tight and two-sided: the cure gate's projection window + # reaches only ~1 cm past the nominal gap, and the params are frozen + # at planning time, so the left block's OWN landing error stacks on + # top of whatever outward bias the sampler adds (a one-sided + # +0-4 mm jitter left a joint outside the window that then never + # cured). left = objs[2] x = state.get(left, "x") + _SPAN_LEN + _ENV.lateral_place_gap + \ - rng.uniform(0.0, 0.004) + rng.uniform(-0.001, 0.001) y = state.get(left, "y") + rng.uniform(-0.003, 0.003) # Gentler landing than the generic places: any landing shift here # is FROZEN into the weld and transfers to the far seat joint, so - # minimize the drop (span center 5-9 mm above resting height). - z = _TABLE + _ENV.span_half_extents[2] + 0.005 + rng.uniform(0.0, 0.004) + # minimize the drop (span center 4-6 mm above resting height). + z = _TABLE + _ENV.span_half_extents[2] + 0.004 + rng.uniform(0.0, 0.002) return np.array([x, y, z, 0.0], dtype=np.float32) @@ -120,15 +129,24 @@ def _seat_span_sampler(state: State, goal: Set[GroundAtom], del goal # objs = [robot, spanA, mid, spanB, legL, legR, siteL, siteR]. The # welded row hangs from its grasped MIDDLE span (see PickRow), so - # seating is symmetric: land mid's center on the midpoint of the - # two leg tops and both outer spans arrive over their legs by the - # rigid geometry. (An end grasp put a 20 cm cantilever on the grasp - # constraint; its torsion yawed the far tip ~2-3 cm, enough to - # strike the far leg's edge on the way down and topple it.) + # seating is near-symmetric: land mid's center on the midpoint of + # the two leg tops and both outer spans arrive over their legs by + # the rigid geometry. (An end grasp put a 20 cm cantilever on the + # grasp constraint; its torsion yawed the far tip ~2-3 cm, enough + # to strike the far leg's edge on the way down and topple it.) + # Placement errors frozen into the welds make the row slightly + # asymmetric about mid, so center the ROW -- the midpoint of the + # outer spans' actual centers -- over the legs, not mid itself; + # otherwise the whole frozen offset lands on one seat joint. + span_a, mid, span_b = objs[1], objs[2], objs[3] + row_dx = (state.get(span_a, "x") + state.get(span_b, "x")) / 2 - \ + state.get(mid, "x") + row_dy = (state.get(span_a, "y") + state.get(span_b, "y")) / 2 - \ + state.get(mid, "y") leg_l, leg_r = objs[4], objs[5] - x = (state.get(leg_l, "x") + state.get(leg_r, "x")) / 2 + \ + x = (state.get(leg_l, "x") + state.get(leg_r, "x")) / 2 - row_dx + \ rng.uniform(-0.003, 0.003) - y = (state.get(leg_l, "y") + state.get(leg_r, "y")) / 2 + \ + y = (state.get(leg_l, "y") + state.get(leg_r, "y")) / 2 - row_dy + \ rng.uniform(-0.003, 0.003) # Release height from STATIC task geometry only. Samplers run at # planning time on predicted states, so live robot-relative reads From e385fe27fba6d6a2180523191ba9117c858c4674 Mon Sep 17 00:00:00 2001 From: Yichao Liang Date: Sun, 16 Aug 2026 17:07:31 -0400 Subject: [PATCH 03/30] bridge: recalibrate GT-sim cure gates so welds can latch The fully-observable GT simulator softens its cure-alignment gates with sigmoids for the fitting Jacobian, using the shared SOFT_EPS (0.02, sized for 5-15 cm thresholds) on the bridge's mm-scale windows. That capped a geometrically PERFECT butt joint's gate weight at ~0.33 and a perfect seat at ~0.77 -- and since the cure counter evolves as prog = w * (cure + 1), which fixed-points at w / (1 - w), curing could NEVER reach the latch threshold (25) for ANY geometry. Welding was impossible in the simulator while trivial in the env: all three agent_oracle_hybrid_sim attempts independently mastered legs, glue, and adjacency in the sandbox, then dead-ended on Attached with cure crawling at ~3e-4/step toward a fixed point of 0.5. Recalibrate with a bridge-local GATE_EPS of 1 mm: in-window joints saturate to w ~= 1 and latch in ~cure_threshold steps (matching the env's hard counter), the boundary keeps a +-4 mm differentiable band, and the sim's effective window sits ~3 mm inside the env's -- always conservative, never the reverse. New unit tests pin the latch behavior (the coverage gap that let this ship): in-window butt and stacked joints latch on schedule, out-of-window joints never do. --- .../bridge/gt_simulator.py | 27 ++-- .../test_bridge_gt_simulator.py | 133 ++++++++++++++++++ 2 files changed, 152 insertions(+), 8 deletions(-) create mode 100644 tests/code_sim_learning/test_bridge_gt_simulator.py diff --git a/predicators/ground_truth_models/bridge/gt_simulator.py b/predicators/ground_truth_models/bridge/gt_simulator.py index 243afb5b3..a4fb51514 100644 --- a/predicators/ground_truth_models/bridge/gt_simulator.py +++ b/predicators/ground_truth_models/bridge/gt_simulator.py @@ -30,8 +30,8 @@ from predicators.code_sim_learning.commands import CommandBuffer from predicators.code_sim_learning.fit_space import ParamSpec -from predicators.code_sim_learning.utils import SOFT_EPS, Params, \ - ResidualUpdate, objs_by_type, sigmoid +from predicators.code_sim_learning.utils import Params, ResidualUpdate, \ + objs_by_type, sigmoid from predicators.ground_truth_models import GroundTruthSimulatorFactory from predicators.settings import CFG from predicators.structs import Object, State @@ -51,6 +51,17 @@ BLOCK_HALF = (0.05, 0.025, 0.025) GLUE_FACES = ("top", "end_a", "end_b") ATTACH_SLOTS = ("top", "bottom", "end_a", "end_b") +# Sigmoid sharpness for the mm-scale alignment gates below. The shared +# ``SOFT_EPS`` (0.02, sized for 5-15 cm thresholds) is 2-20x the gate +# windows here, which capped a PERFECT butt joint's weight at ~0.33 and +# a perfect seat at ~0.77 -- and since ``prog = w * (cure + 1)`` +# fixed-points at ``w / (1 - w)``, curing could NEVER reach the latch +# threshold (25) for any geometry. At 1 mm an in-window joint saturates +# to w ~= 1 (latching in ~cure_threshold steps, matching the env's hard +# counter) while the boundary keeps a +-4 mm differentiable band for +# the fitting Jacobian; the sim's effective window is ~3 mm inside the +# env's hard window -- conservative, never the reverse. +GATE_EPS = 0.001 # Local (normal axis, sign) per face / attachment slot. FACE_AXES = {"top": (2, 1.0), "end_a": (0, -1.0), "end_b": (0, 1.0)} SLOT_AXES = {**FACE_AXES, "bottom": (2, -1.0)} @@ -126,10 +137,10 @@ def _top_mate_weight(state: State, other: Object, blk: Object, if _stands(state, other): # Leg on leg: circular xy alignment. return sigmoid( - (params["stack_align_tol"] - float(np.hypot(dx, dy))) / SOFT_EPS) + (params["stack_align_tol"] - float(np.hypot(dx, dy))) / GATE_EPS) # Span seated on leg: the leg under the span's footprint. - x_w = sigmoid((params["seat_x_window"] - abs(dx)) / SOFT_EPS) - y_w = sigmoid((SEAT_Y_TOL - abs(dy)) / SOFT_EPS) + x_w = sigmoid((params["seat_x_window"] - abs(dx)) / GATE_EPS) + y_w = sigmoid((SEAT_Y_TOL - abs(dy)) / GATE_EPS) return x_w * y_w @@ -147,9 +158,9 @@ def _end_mate_weight(state: State, blk: Object, face: str, other: Object, proj = dx * dx_dir + dy * dy_dir perp = abs(-dx * dy_dir + dy * dx_dir) ext = BLOCK_HALF[FACE_AXES[face][0]] + _half(state, other)[0] - proj_w = sigmoid((0.012 - (proj - ext)) / SOFT_EPS) * \ - sigmoid(((proj - ext) + 0.01) / SOFT_EPS) - perp_w = sigmoid((params["lateral_perp_tol"] - perp) / SOFT_EPS) + proj_w = sigmoid((0.012 - (proj - ext)) / GATE_EPS) * \ + sigmoid(((proj - ext) + 0.01) / GATE_EPS) + perp_w = sigmoid((params["lateral_perp_tol"] - perp) / GATE_EPS) return proj_w * perp_w diff --git a/tests/code_sim_learning/test_bridge_gt_simulator.py b/tests/code_sim_learning/test_bridge_gt_simulator.py new file mode 100644 index 000000000..4d34fa9c1 --- /dev/null +++ b/tests/code_sim_learning/test_bridge_gt_simulator.py @@ -0,0 +1,133 @@ +"""Test the bridge GT hybrid simulator's glue-cure-latch residual. + +The curing gates are sigmoid-softened so the residual is differentiable +in the threshold parameters, and the cure counter evolves as ``prog = w +* (cure + 1)``, which fixed-points at ``w / (1 - w)``. That makes the +latch EXQUISITELY sensitive to the gate sharpness: with the shared +``SOFT_EPS`` (0.02, sized for 5-15 cm thresholds) on these mm-scale +windows, a geometrically PERFECT butt joint capped at w ~= 0.33 (fixed +point 0.49 against a latch threshold of 25), so welding was impossible +in the simulator while trivial in the env -- and every agent planning +against the simulator dead-ended on ``Attached``. These tests pin the +latch behavior directly on hand-built states: an in-window glued joint +must latch in about ``cure_threshold`` steps, and an out-of-window one +must never latch. +""" + +import numpy as np + +from predicators import utils +from predicators.code_sim_learning.utils import apply_rules, \ + has_physics_rules, merge_updates +from predicators.ground_truth_models import get_gt_simulator +from predicators.structs import Object, State, Type + +_GLUE_FACES = ("top", "end_a", "end_b") +_ATTACH_SLOTS = ("top", "bottom", "end_a", "end_b") +_BLOCK_TYPE = Type("block", + ["x", "y", "z", "roll", "pitch", "yaw", "is_held"] + + [f"glue_{f}" + for f in _GLUE_FACES] + [f"cure_{f}" + for f in _GLUE_FACES] + + [f"attached_{s}" for s in _ATTACH_SLOTS]) +# Fixed block-index order matching gt_simulator._block_index. +_BLOCK_IDX = {"leg0": 0, "leg1": 1, "span0": 2, "span1": 3, "span2": 4} +_TABLE_Z = 0.4 +_SPAN_HALF = (0.05, 0.025, 0.025) + + +def _make_block(name, x, y, z): + obj = Object(name, _BLOCK_TYPE) + feats = {f: 0.0 for f in _BLOCK_TYPE.feature_names} + feats.update(x=x, y=y, z=z) + for slot in _ATTACH_SLOTS: + feats[f"attached_{slot}"] = -1.0 + return obj, np.array([feats[f] for f in _BLOCK_TYPE.feature_names], + dtype=np.float32) + + +def _make_state(blocks): + return State(dict(blocks)) + + +def _bridge_sim(): + utils.reset_config({"env": "pybullet_bridge", "seed": 0}) + rules, specs, _ = get_gt_simulator("pybullet_bridge") + params = {s.name: s.init_value for s in specs} + return rules, params + + +def _roll_until_latched(state, rules, params, blk, slot, max_steps): + """Apply the residual rules until ``blk``'s ``slot`` latches; return the + step index or None.""" + for step in range(max_steps): + updates = apply_rules(state, rules, params) + state = merge_updates(state, updates) + if state.get(blk, f"attached_{slot}") >= 0: + return step, state + return None, state + + +def test_bridge_gt_simulator_loads(): + """The factory registry resolves pybullet_bridge to the FO simulator.""" + rules, params = _bridge_sim() + assert [r.__name__ for r in rules] == \ + ["_glue_application", "_curing", "_welding"] + assert params["cure_threshold"] == 25.0 + # The welding rule acts through the physics-command channel, so the + # fitting stack must route it to the rollout objective. + assert has_physics_rules(rules) + + +def test_butt_joint_cures_and_latches(): + """A glued, butted, resting span pair latches in ~cure_threshold steps. + + 3 mm joint gap: comfortably inside the [-10 mm, +12 mm] projection + window, matching what the oracle place sampler produces. + """ + rules, params = _bridge_sim() + span0, arr0 = _make_block("span0", 0.60, 1.14, _TABLE_Z + _SPAN_HALF[2]) + span1, arr1 = _make_block("span1", 0.703, 1.14, _TABLE_Z + _SPAN_HALF[2]) + state = _make_state([(span0, arr0), (span1, arr1)]) + state.set(span0, "glue_end_b", 1.0) + latch_step, state = _roll_until_latched(state, rules, params, span0, + "end_b", 40) + assert latch_step is not None, "in-window glued joint never latched" + # The env's hard counter latches on step cure_threshold; the soft + # gates may cost a few extra steps but not more. + assert latch_step <= params["cure_threshold"] + 5 + assert state.get(span0, "attached_end_b") == _BLOCK_IDX["span1"] + assert state.get(span1, "attached_end_a") == _BLOCK_IDX["span0"] + # The latch consumes the glue. + assert state.get(span0, "glue_end_b") == 0.0 + + +def test_stacked_top_joint_cures_and_latches(): + """A glued top face with a block resting on it latches too (the upward-face + mate path).""" + rules, params = _bridge_sim() + span0, arr0 = _make_block("span0", 0.60, 1.14, _TABLE_Z + _SPAN_HALF[2]) + span1, arr1 = _make_block("span1", 0.605, 1.145, + _TABLE_Z + 3 * _SPAN_HALF[2]) + state = _make_state([(span0, arr0), (span1, arr1)]) + state.set(span0, "glue_top", 1.0) + latch_step, state = _roll_until_latched(state, rules, params, span0, "top", + 40) + assert latch_step is not None, "in-window stacked joint never latched" + assert latch_step <= params["cure_threshold"] + 5 + assert state.get(span0, "attached_top") == _BLOCK_IDX["span1"] + assert state.get(span1, "attached_bottom") == _BLOCK_IDX["span0"] + + +def test_out_of_window_joint_never_latches(): + """A glued joint with a 2 cm gap (outside the +12 mm window) must not + cure -- the gates gate, they don't just delay.""" + rules, params = _bridge_sim() + span0, arr0 = _make_block("span0", 0.60, 1.14, _TABLE_Z + _SPAN_HALF[2]) + span1, arr1 = _make_block("span1", 0.72, 1.14, _TABLE_Z + _SPAN_HALF[2]) + state = _make_state([(span0, arr0), (span1, arr1)]) + state.set(span0, "glue_end_b", 1.0) + latch_step, state = _roll_until_latched(state, rules, params, span0, + "end_b", 80) + assert latch_step is None + assert state.get(span0, "cure_end_b") < 1.0 From 46cffb68e2abad61f21b1b94ba58de95d4899fd5 Mon Sep 17 00:00:00 2001 From: Yichao Liang Date: Sun, 16 Aug 2026 17:07:45 -0400 Subject: [PATCH 04/30] bridge: robustify carried-assembly planning and demo-time placement Fixes for the three demo/execution failures the launched agent_oracle_hybrid_sim runs exposed on train-task instances: - Pose welded partners from the welds' IDEAL frames during motion planning (new env API get_welded_partner_transforms, chained from the constraints' snapped frames), instead of capturing live poses: the carried row swings like a pendulum after a lift, and a live snapshot froze an outer span 19 mm low, failing a seat-descend goal the settled assembly clears. The seat release clearance returns to the long-tested 1.2 cm. - Execute every planned waypoint for bridge (path subsample ratio 2 -> 1): a carried span travels 5-8 cm per physics step between subsampled waypoints, and that corner-cutting swept a just-picked span through a standing leg's top corner (a 0.3 mm graze topples the 2:1 leg, and a toppled leg is unrecoverable -- PlaceLegAtSite requires Standing and nothing re-erects one). - held_bystander_clearance of 1 cm for bridge skills: the carried block lags the end effector's swings by up to centimetres, so plans keep a real berth from pass-by bodies; the planner's endpoint exemption keeps butt joints and seating plannable. - Mirror the current yaml in the oracle bridge smoke test's flattened config (contact margin -0.005, subsample 1): welded-partner checking made a by-design 2 mm frozen-offset graze at the seat goal visible, which the stale 1 mm default margin rejected. Oracle E2E after the fixes: train tasks (the demo path) 3/3 on seeds 0-2; test tasks 2/3 with the remaining miss a marginal seat-landing draw that four probe variants of the identical task all clear. --- predicators/envs/pybullet_bridge.py | 40 ++++++++++++++ .../ground_truth_models/bridge/options.py | 9 +++ .../ground_truth_models/bridge/processes.py | 7 ++- .../skill_factories/base.py | 55 +++++++++++++------ scripts/configs/predicatorv3/envs/all.yaml | 9 ++- .../test_oracle_process_planning_bridge.py | 11 +++- tests/envs/test_pybullet_bridge.py | 7 +++ 7 files changed, 119 insertions(+), 19 deletions(-) diff --git a/predicators/envs/pybullet_bridge.py b/predicators/envs/pybullet_bridge.py index 4d85aac52..637120cc6 100644 --- a/predicators/envs/pybullet_bridge.py +++ b/predicators/envs/pybullet_bridge.py @@ -985,6 +985,46 @@ def _sync_welds_to_state(self, state: State) -> None: if key not in self._weld_constraints: self._create_weld(body_a, body_b, ideal_dz=ideal_dz) + def get_welded_partner_transforms( + self, body_id: int + ) -> Dict[int, Tuple[Tuple[float, ...], Tuple[float, ...]]]: + """Ideal ``(position, orientation)`` of every transitively welded + partner RELATIVE to ``body_id``, chained from the weld constraints' + snapped frames. + + Consumed by the skill-factory motion planner to pose welded + partners of the held object. The constraint frames are the + settled geometry the physical assembly returns to; live partner + poses instead snapshot whatever pendulum transient the carried + assembly is mid-swing through (an outer span was captured 19 mm + low right after a lift), which poisons every collision check + that reuses the capture. + """ + out: Dict[int, Tuple[Tuple[float, ...], Tuple[float, ...]]] = {} + identity = ((0.0, 0.0, 0.0), (0.0, 0.0, 0.0, 1.0)) + frontier: List[int] = [body_id] + transforms = {body_id: identity} + while frontier: + current = frontier.pop() + for key, cid in self._weld_constraints.items(): + if current not in key: + continue + (other, ) = key - {current} + if other in transforms: + continue + info = p.getConstraintInfo( + cid, physicsClientId=self._physics_client_id) + parent_id, rel = info[0], (info[6], info[8]) + step_tf = rel if current == parent_id else \ + p.invertTransform(rel[0], rel[1]) + base = transforms[current] + tf = p.multiplyTransforms(base[0], base[1], step_tf[0], + step_tf[1]) + transforms[other] = tf + out[other] = tf + frontier.append(other) + return out + def get_welded_partner_ids(self, body_id: int) -> Set[int]: """All body ids rigidly welded (transitively) to ``body_id``. diff --git a/predicators/ground_truth_models/bridge/options.py b/predicators/ground_truth_models/bridge/options.py index 1525b4ee7..651a24d6a 100644 --- a/predicators/ground_truth_models/bridge/options.py +++ b/predicators/ground_truth_models/bridge/options.py @@ -249,4 +249,13 @@ def _build_skill_config( env_cls.robot_init_z), transport_z=env_cls.transport_z, simulator=simulator, + # The carried block lags the end effector's mid-path swings + # by up to centimetres, and the standing legs (2:1 aspect) + # topple from a fraction-of-a-mm graze, so plans must keep + # a real berth between the carried block and bodies the + # path only passes by. Bodies within this clearance of the + # held object at a path ENDPOINT (butt-joint neighbors, + # seat legs, glue targets) are exempted by the planner, so + # deliberately tight placements stay plannable. + held_bystander_clearance=0.01, ) diff --git a/predicators/ground_truth_models/bridge/processes.py b/predicators/ground_truth_models/bridge/processes.py index 150f1dc20..d6906f0ad 100644 --- a/predicators/ground_truth_models/bridge/processes.py +++ b/predicators/ground_truth_models/bridge/processes.py @@ -154,7 +154,12 @@ def _seat_span_sampler(state: State, goal: Set[GroundAtom], # blew past the release_z bound, and crash-dropped the assembly). # mid's center = leg top + span half-thickness + ~1.2 cm drop # clearance for the rigid assembly to self-level. A 2 cm drop let - # an offset end strike the far leg hard enough to topple it. + # an offset end free-fall onto the far leg hard enough to topple + # it, so keep the drop minimal. (The descend's collision check + # poses the welded partners from the welds' IDEAL frames, so the + # carried row's pendulum transients -- up to ~2 cm at an outer + # span right after a lift -- cannot fail the goal check; the + # settled row is what lands.) release_z = _TABLE + _LEG_H + _ENV.span_half_extents[2] + 0.012 return np.array([x, y, release_z, 0.0], dtype=np.float32) diff --git a/predicators/ground_truth_models/skill_factories/base.py b/predicators/ground_truth_models/skill_factories/base.py index 6cd6e6ca6..abdd84e6f 100644 --- a/predicators/ground_truth_models/skill_factories/base.py +++ b/predicators/ground_truth_models/skill_factories/base.py @@ -985,8 +985,13 @@ def _sim_collision_context( # make every transport plan collide immediately. Remove them from # the obstacle set and instead hand them to the motion planner as # rigid attachments of the held object (posed with the arm and - # collision-checked like the held object itself), capturing their - # end-effector-relative transforms from the just-reset simulator. + # collision-checked like the held object itself). Their + # end-effector-relative transforms chain the held object's grasp + # transform with the welds' IDEAL relative frames -- live partner + # poses would snapshot whatever pendulum transient the carried + # assembly is mid-swing through (an outer span was captured + # 19 mm low right after a lift, failing a descend goal the + # settled assembly clears). held_attachments: Dict[int, Any] = {} if held_object is not None: get_welded = getattr(sim, "get_welded_partner_ids", None) @@ -995,20 +1000,38 @@ def _sim_collision_context( collision_bodies -= welded_ids if welded_ids: client = sim._physics_client_id # pylint: disable=protected-access - planning_robot = sim._pybullet_robot # pylint: disable=protected-access - planning_robot.set_joints(pb_state.joint_positions) - world_to_base_link = get_link_state( - planning_robot.robot_id, - planning_robot.end_effector_id, - physics_client_id=client).com_pose - base_link_to_world = p.invertTransform( - world_to_base_link[0], world_to_base_link[1]) - for welded_id in welded_ids: - world_to_obj = p.getBasePositionAndOrientation( - welded_id, physicsClientId=client) - held_attachments[welded_id] = p.multiplyTransforms( - base_link_to_world[0], base_link_to_world[1], - world_to_obj[0], world_to_obj[1]) + held_to_base_link = sim._held_obj_to_base_link # pylint: disable=protected-access + get_transforms = getattr(sim, + "get_welded_partner_transforms", + None) + if get_transforms is not None and \ + held_to_base_link is not None: + base_link_to_held = p.invertTransform( + held_to_base_link[0], held_to_base_link[1]) + held_to_partners = get_transforms(held_object) + for welded_id, held_to_obj in held_to_partners.items(): + held_attachments[welded_id] = p.multiplyTransforms( + base_link_to_held[0], base_link_to_held[1], + held_to_obj[0], held_to_obj[1]) + else: + # Fallback for envs without ideal weld frames: + # live-pose capture relative to the end effector. + planning_robot = sim._pybullet_robot # pylint: disable=protected-access + planning_robot.set_joints(pb_state.joint_positions) + world_to_base_link = get_link_state( + planning_robot.robot_id, + planning_robot.end_effector_id, + physics_client_id=client).com_pose + base_link_to_world = p.invertTransform( + world_to_base_link[0], world_to_base_link[1]) + for welded_id in welded_ids: + world_to_obj = p.getBasePositionAndOrientation( + welded_id, physicsClientId=client) + held_attachments[welded_id] = \ + p.multiplyTransforms( + base_link_to_world[0], + base_link_to_world[1], world_to_obj[0], + world_to_obj[1]) # 4b. Add tables if present. if hasattr(sim, '_table_ids'): diff --git a/scripts/configs/predicatorv3/envs/all.yaml b/scripts/configs/predicatorv3/envs/all.yaml index ae76a0811..3407dea95 100644 --- a/scripts/configs/predicatorv3/envs/all.yaml +++ b/scripts/configs/predicatorv3/envs/all.yaml @@ -410,4 +410,11 @@ ENVS: # margin turns those into unrecoverable BiRRT start/goal # rejections. pybullet_birrt_contact_margin: -0.005 - pybullet_birrt_path_subsample_ratio: 2 + # Execute EVERY planned waypoint (no subsampling): a carried span + # can travel 5-8 cm per physics step between subsampled + # waypoints, and that corner-cutting swept a just-picked span + # through a standing leg's top corner (a 0.3 mm graze topples the + # 2:1 leg). The bridge's standing legs are the tippiest obstacles + # in the codebase; path fidelity is worth the ~2x steps per + # motion-planned phase (episodes stay far under the horizon). + pybullet_birrt_path_subsample_ratio: 1 diff --git a/tests/approaches/test_oracle_process_planning_bridge.py b/tests/approaches/test_oracle_process_planning_bridge.py index c0dbc6b27..4c952f406 100644 --- a/tests/approaches/test_oracle_process_planning_bridge.py +++ b/tests/approaches/test_oracle_process_planning_bridge.py @@ -39,7 +39,16 @@ def _oracle_bridge_config() -> dict: # --- env: bridge from envs/all.yaml --- "env": "pybullet_bridge", "horizon": 3000, - "pybullet_birrt_path_subsample_ratio": 2, + # Execute every planned waypoint: subsampled execution cuts + # corners the plan cleared, and a carried span grazing a + # standing 2:1 leg by a fraction of a mm topples it. + "pybullet_birrt_path_subsample_ratio": 1, + # The packed staging grid leaves ~1-2 cm clearances that + # stochastically dip into 2-3 mm grazes (e.g. a welded row's + # frozen offsets against a seat leg at the descend goal); the + # default 1 mm margin turns those into unrecoverable BiRRT + # start/goal rejections. + "pybullet_birrt_contact_margin": -0.005, # Each Wait ends on the FIRST atom change, so a plan waiting on # several concurrent cures can need a cheap replan for the tail # (which reduces to "Wait until the remaining joint cures"). diff --git a/tests/envs/test_pybullet_bridge.py b/tests/envs/test_pybullet_bridge.py index 1dc120867..af251231e 100644 --- a/tests/envs/test_pybullet_bridge.py +++ b/tests/envs/test_pybullet_bridge.py @@ -92,6 +92,13 @@ def test_glue_cure_weld_lifecycle(env_and_task): assert env._Attached_holds(final, [leg1, leg0]) assert not env._Loose_holds(final, [leg0]) assert env.get_welded_partner_ids(leg0.id) == {leg1.id} + # The ideal partner transform comes from the weld's SNAPPED frame: + # leg1 stacked on standing leg0 sits one block-length up (0.1 m), + # independent of any pendulum transient in the live poses. + transforms = env.get_welded_partner_transforms(leg0.id) + assert set(transforms) == {leg1.id} + rel_pos, _ = transforms[leg1.id] + assert abs(np.linalg.norm(rel_pos) - 2 * env.leg_half_extents[2]) < 0.02 # 3. Fresh reset removes the weld and restores default features. env._set_state(task.init) From 04483de328626023520ceb5768220128428c4776 Mon Sep 17 00:00:00 2001 From: Yichao Liang Date: Mon, 17 Aug 2026 07:47:10 -0400 Subject: [PATCH 05/30] bridge: stop glue/cure/attached sim_data leaking across env instances Glue, cure, and attachment features live in Object.sim_data, which is stored on the Object INSTANCE. States routinely cross env instances (option-model resets, refinement rollouts, fresh per-episode test envs) carrying the source env's Object instances, and the bridge env read and wrote those features through whatever instance was at hand: _set_domain_specific_state wrote onto the incoming state's objects, and _domain_specific_step / _latch_joint wrote onto the objects of self._objects, which _set_state points at the incoming instances. Net effect: a sim rollout in one env silently mutated another env's hidden glue state. Observed in the seed-1/seed-2 agent run post- mortems as impossible values in the real env's final state dump (glue flags the execution monitor had correctly seen missing, and soft-cure floats like 1.55e-142 that the env's integer cure ticks cannot produce) written there by suffix-replan refinement rollouts on the option-model env. It would also have poisoned real execution resuming after any successful replan, and it made the wet-patch visuals (which read the env-owned instances) disagree with the imported state. Fix: resolve every sim_data access to the env-owned canonical block first. _attr becomes an instance method that canonicalizes via the new _own_block (name lookup into self._blocks), and all raw setattr sites go through the new _set_attr counterpart. Adds a regression test that imports a glued state into a second env instance and asserts the first env's observation stays clean (fails before this change). --- predicators/envs/pybullet_bridge.py | 58 ++++++++++++++++++++--------- tests/envs/test_pybullet_bridge.py | 41 ++++++++++++++++++++ 2 files changed, 81 insertions(+), 18 deletions(-) diff --git a/predicators/envs/pybullet_bridge.py b/predicators/envs/pybullet_bridge.py index 637120cc6..c6a178ad0 100644 --- a/predicators/envs/pybullet_bridge.py +++ b/predicators/envs/pybullet_bridge.py @@ -585,13 +585,33 @@ def _store_pybullet_bodies(self, pybullet_bodies: Dict[str, Any]) -> None: # ------------------------------------------------------------------------- # Small helpers # ------------------------------------------------------------------------- - @staticmethod - def _attr(blk: Object, name: str, default: float) -> float: - """Read a sim-feature attribute with an explicit None default (0.0 is a - meaningful value for attached_* -- block index 0).""" - val = getattr(blk, name) + def _own_block(self, blk: Object) -> Object: + """This env's canonical instance of ``blk``, matched by name. + + Glue/cure/attached live in ``Object.sim_data``, which is stored + on the INSTANCE. States routinely cross env instances (option- + model resets, refinement rollouts, fresh test envs) carrying the + source env's Object instances, so reading or writing sim_data + through a state-derived block would silently share hidden glue + state between envs. Every sim_data access therefore resolves to + the env-owned instance first. + """ + idx = self._block_index.get(blk.name) + return self._blocks[idx] if idx is not None else blk + + def _attr(self, blk: Object, name: str, default: float) -> float: + """Read a sim-feature attribute off this env's own instance. + + The None default is explicit because 0.0 is a meaningful value + for attached_* (block index 0). + """ + val = getattr(self._own_block(blk), name) return float(val) if val is not None else default + def _set_attr(self, blk: Object, name: str, value: float) -> None: + """Write a sim-feature attribute onto this env's own instance.""" + setattr(self._own_block(blk), name, value) + @classmethod def _is_leg_shaped(cls, blk: Object) -> bool: """Task-generation ROLE by name (which blocks start standing). @@ -825,18 +845,19 @@ def _set_domain_specific_state(self, state: State) -> None: blocks = state.get_objects(self._block_type) for blk in blocks: for face in GLUE_FACES: - setattr(blk, f"glue_{face}", state.get(blk, f"glue_{face}")) + self._set_attr(blk, f"glue_{face}", + state.get(blk, f"glue_{face}")) if f"cure_{face}" in blk.type.feature_names: - setattr(blk, f"cure_{face}", - state.get(blk, f"cure_{face}")) + self._set_attr(blk, f"cure_{face}", + state.get(blk, f"cure_{face}")) else: priv = state.privileged or {} - setattr( + self._set_attr( blk, f"cure_{face}", float(priv.get(blk.name, {}).get(f"cure_{face}", 0.0))) for slot in ATTACH_SLOTS: - setattr(blk, f"attached_{slot}", - self._attached_value(state, blk, slot)) + self._set_attr(blk, f"attached_{slot}", + self._attached_value(state, blk, slot)) # Colors are task-assigned features; the base env never # writes them to PyBullet, so apply them here. if blk.id is not None: @@ -1077,7 +1098,7 @@ def _domain_specific_step(self) -> None: best_dist = dist if best is not None: blk, face = best - setattr(blk, f"glue_{face}", 1.0) + self._set_attr(blk, f"glue_{face}", 1.0) # 2. Curing: wet faces in aligned resting contact tick; at the # threshold the joint latches irreversibly and welds. @@ -1089,10 +1110,10 @@ def _domain_specific_step(self) -> None: continue mate = self._find_mate(state, blk, face) if mate is None: - setattr(blk, f"cure_{face}", 0.0) + self._set_attr(blk, f"cure_{face}", 0.0) continue cure = self._attr(blk, f"cure_{face}", 0.0) + 1.0 - setattr(blk, f"cure_{face}", cure) + self._set_attr(blk, f"cure_{face}", cure) if cure >= self.cure_threshold: self._latch_joint(state, blk, face, mate) @@ -1204,10 +1225,11 @@ def _latch_joint(self, state: State, blk: Object, face: str, # than corrupt the attachment graph (cure stays at the # threshold, so this re-checks every step). return - setattr(blk, f"attached_{face}", float(self._block_index[mate.name])) - setattr(mate, f"attached_{mate_slot}", - float(self._block_index[blk.name])) - setattr(blk, f"glue_{face}", 0.0) + self._set_attr(blk, f"attached_{face}", + float(self._block_index[mate.name])) + self._set_attr(mate, f"attached_{mate_slot}", + float(self._block_index[blk.name])) + self._set_attr(blk, f"glue_{face}", 0.0) assert blk.id is not None and mate.id is not None if self._face_world_dir(state, blk, face)[2] > np.cos(np.pi / 4): # The mate rests on blk's upward face: a vertical joint. diff --git a/tests/envs/test_pybullet_bridge.py b/tests/envs/test_pybullet_bridge.py index af251231e..c9ea525a6 100644 --- a/tests/envs/test_pybullet_bridge.py +++ b/tests/envs/test_pybullet_bridge.py @@ -113,6 +113,47 @@ def test_glue_cure_weld_lifecycle(env_and_task): assert len(env._weld_constraints) == 1 +def test_sim_data_isolated_between_env_instances(env_and_task): + """Glue/cure/attached written by one env instance must never leak into + another env instance through shared State Object instances. + + Regression: those features live in ``Object.sim_data`` (stored on + the instance), and states routinely cross env instances carrying + the source env's objects (option-model resets, refinement + rollouts). ``_set_domain_specific_state`` used to write through the + incoming instances, so a sim rollout's glue/cure values bled into + the real env's next observation (observed as impossible soft-cure + floats in a real env's post-mortem state dump). + """ + env, task = env_and_task + env._set_state(task.init) + src_state = env._get_state() + leg0 = next(b for b in src_state.get_objects(env._block_type) + if b.name == "leg0") + assert src_state.get(leg0, "glue_end_b") == 0.0 + + from predicators.envs.pybullet_bridge import \ + PyBulletBridgeEnv # pylint: disable=import-outside-toplevel + other = PyBulletBridgeEnv(use_gui=False) + try: + glued = src_state.copy() + glued.set(leg0, "glue_end_b", 1.0) + glued.set(leg0, "cure_end_b", 3.0) + # The other env must import the features into its OWN blocks... + other._set_state(glued) + other_state = other._get_state() + assert other_state.get(leg0, "glue_end_b") == 1.0 + assert other_state.get(leg0, "cure_end_b") == 3.0 + # ...without touching this env's blocks (src_state's Object + # instances belong to ``env``). + fresh = env._get_state() + assert fresh.get(leg0, "glue_end_b") == 0.0 + assert fresh.get(leg0, "cure_end_b") == 0.0 + finally: + import pybullet as p # pylint: disable=import-outside-toplevel + p.disconnect(other._physics_client_id) + + def test_seat_weld_holds_pose(env_and_task): """A cured seat joint (lying span welded onto a STANDING leg's top) must hold the assembly rigidly at the seated pose. From c9d24fb59f74bafef8ead45500da44b3d3171599 Mon Sep 17 00:00:00 2001 From: Yichao Liang Date: Mon, 17 Aug 2026 09:30:18 -0400 Subject: [PATCH 06/30] skills: guarded settle-to-contact release; bridge: kill drop-settle scatter Place releases were open-loop drops: descend to release_z, open, and let the block free-fall the last few mm. The bounce-and-slide settle scatters the landing by 1-3 mm run to run, and this domain's margins are exactly that size (butt-joint cure windows, 2:1 legs that topple from sub-mm grazes, the seat landing). That scatter was the bridge demonstrator's dominant flakiness. Place skills gain an optional guarded SettleToContact phase (settle_to_contact_depth): after the collision-checked descent, an incremental-IK contact stroke lowers the held object until the held ASSEMBLY (the object or anything welded to it, discovered by BFS over the client's fixed constraints) first touches a body outside itself, then releases at essentially zero gap. The stroke runs as a gentle stroke (new Phase.max_step_norm): 3 mm steps bound the post-contact overshoot, a joint-jump guard suppresses IK branch flips (single-shot panda IK once answered a plain 2 cm descent with a wrist-flipped solution whose retreat batted the released block 9 cm across the table), and the incremental-IK stall abort is armed as the escape hatch. make_move_to_phase forwards terminal_fn and max_step_norm. Bridge wiring and margin retuning on top: - settle_to_contact_depth=0.03 on Place; sampler drop clearances cut to 2-3 mm (now descend-goal clearances, not free-fall heights). - Seat descend clearance raised 12 -> 20 mm: extra clearance now costs nothing, and the descend-goal check inherits the held span's LIVE pitch through the grasp transform (a carried row rides at ~0.1 rad, hanging an outer span up to 19 mm low), which at 12 mm collided with a leg top and killed otherwise-sound seat goals. - PickBottle lift raised to 3 cm and Place's first phase allows shallow held start contacts; the bridge config deepens the shallow margin to -12 mm: post-pick reconstruction artifacts occasionally model the held object up to ~9 mm into the surface it was just lifted off, rejecting the next Place's start config. Demonstrator sweep (4 seeds x 3 train + 3 test): 24/24 solved under the final configuration, up from 21/24; the three old failures (two seat grazes, one pick handoff) and the scatter class are gone. Full suite: 1483 passed. New physics regression test pins the contract: a release_z 8 mm high still lands the block at resting height, on target, without spin. --- .../ground_truth_models/bridge/options.py | 22 ++- .../ground_truth_models/bridge/processes.py | 63 ++++---- .../skill_factories/base.py | 82 ++++++++-- .../skill_factories/move_to.py | 14 +- .../skill_factories/place.py | 145 +++++++++++++++++- scripts/configs/predicatorv3/envs/all.yaml | 8 + .../test_oracle_process_planning_bridge.py | 4 + tests/envs/test_pybullet_bridge.py | 61 ++++++++ 8 files changed, 347 insertions(+), 52 deletions(-) diff --git a/predicators/ground_truth_models/bridge/options.py b/predicators/ground_truth_models/bridge/options.py index 651a24d6a..c73f0a26c 100644 --- a/predicators/ground_truth_models/bridge/options.py +++ b/predicators/ground_truth_models/bridge/options.py @@ -30,8 +30,12 @@ # Place params: canonical (x, y, release_z, yaw) order. All three # position params are HELD-OBJECT coordinates (live-compensated, see # compensate_held_offset / compensate_held_z below): release_z is the -# held object's CENTER height at release -- a span dropped on the -# table releases at ~0.435, a span seated on the leg tops at ~0.545. +# held object's CENTER height at the END OF THE DESCENT -- the skill +# then settles to first contact before releasing (see +# settle_to_contact_depth below), so release_z only needs to clear the +# scene; the block touches down with essentially no free fall. A span +# descends over the table to ~0.428, a span seated on the leg tops to +# ~0.545. _BRIDGE_PLACE_PARAMS = [ ("target_x (world x position for the held object)", PyBulletBridgeEnv.workspace_x_lo, PyBulletBridgeEnv.workspace_x_hi), @@ -150,6 +154,11 @@ def _get_bottle_grasp_pose( get_target_pose_fn=_get_bottle_grasp_pose, approach_open=True, anchor_lift=True, + # The default 1 cm lift is within the move-to acceptance + # radius, so the pick can end with the bottle still at + # table height; grasp-constraint droop then models it in + # table contact at the next option's planning start. + lift_dz=0.03, ) # -- Place (generic; geometry via params) --------------------------- @@ -172,6 +181,15 @@ def _get_bottle_grasp_pose( # forever). compensate_held_offset=True, compensate_held_z=True, + # Guarded release: after the (collision-checked) descent to + # release_z, settle straight down to FIRST contact of the + # held assembly before opening. Drop-settle scatter is what + # flips this domain's tight tolerances (a butt joint's cure + # window, a 2:1 leg's sub-mm topple threshold, the seat's + # chaotic landing). 3 cm covers the largest sampler descend + # clearance (the seat's 20 mm) with margin; table places + # settle only their 2-3 mm. + settle_to_contact_depth=0.03, ) # -- MoveTo (generic move-through-pose) ------------------------------ diff --git a/predicators/ground_truth_models/bridge/processes.py b/predicators/ground_truth_models/bridge/processes.py index d6906f0ad..ab0394f7a 100644 --- a/predicators/ground_truth_models/bridge/processes.py +++ b/predicators/ground_truth_models/bridge/processes.py @@ -31,15 +31,17 @@ _SPAN_LEN = 2 * _ENV.span_half_extents[0] # 0.10 _SPAN_TH = 2 * _ENV.span_half_extents[2] # 0.05 _TABLE = _ENV.table_height -# Release the held object with its underside ~8 mm above the resting -# surface. Place's release_z is the HELD OBJECT'S CENTER height (the -# skill live-compensates the EE-to-held offset on all axes), so a -# target is simply resting-center + drop -- no grasp-depth or -# IK-residual budgeting. Keep drops SMALL: standing legs topple from -# hard landings (a 2 cm seat drop knocked the far leg over), and butt -# joints freeze landing error into the weld; 8 mm still clears the -# BiRRT contact margin with room for mm-level execution error. -_DROP = 0.008 +# End the collision-checked descent with the held object's underside +# ~3 mm above the resting surface. Place's release_z is the HELD +# OBJECT'S CENTER height (the skill live-compensates the EE-to-held +# offset on all axes), so a target is simply resting-center + this +# clearance -- no grasp-depth or IK-residual budgeting. The skill then +# settles to FIRST CONTACT before opening (settle_to_contact_depth in +# options.py), so this is a descend-goal clearance, not a free-fall +# height: it only needs to keep the BiRRT goal out of contact with +# ~1 mm of IK error to spare. Small is still better -- it shortens the +# unplanned settle stroke. +_DROP = 0.003 def _pick_sampler(state: State, goal: Set[GroundAtom], @@ -90,12 +92,12 @@ def _place_leg_at_site_sampler(state: State, goal: Set[GroundAtom], site = objs[2] x = state.get(site, "x") + rng.uniform(-0.003, 0.003) y = state.get(site, "y") + rng.uniform(-0.003, 0.003) - # Half the generic drop: a standing leg is a 2:1 block, and an - # 8-13 mm drop can land it rocking near its tipping balance -- one - # observed leg leaned ~0.6 deg at release and slowly toppled ~30 - # steps later with nothing touching it. 4-6 mm sheds most of the - # landing energy while still clearing the BiRRT contact margin. - z = _TABLE + _ENV.leg_half_extents[2] + 0.004 + rng.uniform(0.0, 0.002) + # Standing legs are 2:1 blocks that topple from hard landings (an + # 8-13 mm drop once landed a leg rocking near its tipping balance; + # it leaned ~0.6 deg at release and slowly fell ~30 steps later). + # The settle-to-contact release removes the free fall entirely, so + # this is just the descend-goal clearance above resting height. + z = _TABLE + _ENV.leg_half_extents[2] + 0.002 + rng.uniform(0.0, 0.001) return np.array([x, y, z, 0.0], dtype=np.float32) @@ -116,10 +118,11 @@ def _place_next_to_sampler(state: State, goal: Set[GroundAtom], x = state.get(left, "x") + _SPAN_LEN + _ENV.lateral_place_gap + \ rng.uniform(-0.001, 0.001) y = state.get(left, "y") + rng.uniform(-0.003, 0.003) - # Gentler landing than the generic places: any landing shift here - # is FROZEN into the weld and transfers to the far seat joint, so - # minimize the drop (span center 4-6 mm above resting height). - z = _TABLE + _ENV.span_half_extents[2] + 0.004 + rng.uniform(0.0, 0.002) + # Any landing shift here is FROZEN into the weld and transfers to + # the far seat joint; the settle-to-contact release means the block + # touches down with no free fall, so this is just the descend-goal + # clearance above resting height. + z = _TABLE + _ENV.span_half_extents[2] + 0.002 + rng.uniform(0.0, 0.001) return np.array([x, y, z, 0.0], dtype=np.float32) @@ -152,15 +155,19 @@ def _seat_span_sampler(state: State, goal: Set[GroundAtom], # planning time on predicted states, so live robot-relative reads # are stale garbage (a robot-z-based "hang" read the home pose, # blew past the release_z bound, and crash-dropped the assembly). - # mid's center = leg top + span half-thickness + ~1.2 cm drop - # clearance for the rigid assembly to self-level. A 2 cm drop let - # an offset end free-fall onto the far leg hard enough to topple - # it, so keep the drop minimal. (The descend's collision check - # poses the welded partners from the welds' IDEAL frames, so the - # carried row's pendulum transients -- up to ~2 cm at an outer - # span right after a lift -- cannot fail the goal check; the - # settled row is what lands.) - release_z = _TABLE + _LEG_H + _ENV.span_half_extents[2] + 0.012 + # mid's center = leg top + span half-thickness + 2 cm descend + # clearance for the carried rigid assembly. The settle-to-contact + # release lowers the row until an outer span first touches a leg + # top, so extra clearance costs nothing (no free fall to harden the + # landing; the old 12 mm clearance's 2 cm-drop predecessor once + # toppled the far leg). It needs to be generous: the descend-goal + # collision check poses the welded partners from the welds' IDEAL + # frames (flat relative dz), but the whole modeled row inherits the + # held span's LIVE pitch through the grasp transform -- a carried + # row rides at ~0.06-0.1 rad, which hangs an outer span 8-19 mm + # below the held one, and at 12 mm clearance that modeled droop + # collided with a leg top and killed otherwise-sound seat goals. + release_z = _TABLE + _LEG_H + _ENV.span_half_extents[2] + 0.020 return np.array([x, y, release_z, 0.0], dtype=np.float32) diff --git a/predicators/ground_truth_models/skill_factories/base.py b/predicators/ground_truth_models/skill_factories/base.py index abdd84e6f..e162cd746 100644 --- a/predicators/ground_truth_models/skill_factories/base.py +++ b/predicators/ground_truth_models/skill_factories/base.py @@ -338,6 +338,19 @@ class Phase: # open, the target moves further out, and the phase never terminates # short of fully open. anchor_finger_target: bool = False + # Gentle-stroke mode for incremental-IK phases that deliberately seek + # contact (e.g. a place's settle-to-contact). When set, this overrides + # the EE step clamp (meters per step; default config.max_vel_norm), and + # additionally arms two safety rails in _execute_move_ik/_execute_move: + # - a joint-jump guard: a mm-scale EE step never legitimately needs a + # multi-radian joint move, but single-shot IK (the panda path) can + # return a wrist-flipped branch near contact; executing that action + # drags the held object through the scene, so the step is replaced + # by a hold-position action instead; + # - the incremental-IK stall abort (see _check_ik_stall), so a stroke + # pinned by the guard or blocked short of its target fails the + # option cleanly rather than pressing forever. + max_step_norm: Optional[float] = None class PhaseSkill: @@ -576,6 +589,11 @@ def _execute_move(self, phase: Phase, state: State, memory: Dict, if phase.use_motion_planning: return self._execute_move_birrt(phase, state, memory, objects, params) + if phase.max_step_norm is not None: + # Gentle strokes get the stall abort: a stroke pinned by the + # joint-jump guard (or blocked short of its target) must fail + # the option instead of holding position forever. + self._check_ik_stall(phase, state, memory, objects, params) return self._execute_move_ik(phase, state, objects, params) # Mobile-base positioning. Before the first reach of an option, drive the @@ -1402,6 +1420,10 @@ def _check(joints: JointPositions, label: str) -> None: logging.error("[%s/%s] %s", self._name, phase_name, diag) return diagnostics + # Gentle strokes (Phase.max_step_norm): any single arm joint asked to + # move further than this in one step is a branch flip, not tracking. + _ik_joint_jump_max: ClassVar[float] = 0.5 # radians + def _execute_move_ik(self, phase: Phase, state: State, objects: Sequence[Object], params: Array) -> Action: """Execute a MOVE_TO_POSE phase using incremental IK delta-stepping.""" @@ -1411,21 +1433,8 @@ def _execute_move_ik(self, phase: Phase, state: State, current_pose, target_pose, finger_status = phase.target_fn( state, objects, params, self._config) try: - return get_move_end_effector_to_pose_action( - robot=robot, - current_joint_positions=pb_state.joint_positions, - current_pose=current_pose, - target_pose=target_pose, - finger_status=finger_status, - max_vel_norm=self._config.max_vel_norm, - finger_action_nudge_magnitude=( - self._config.finger_action_nudge_magnitude), - validate=self._config.ik_validate, - # Base positioning is handled once per option by - # _maybe_drive_base; keep incremental IK arm-only so the base - # doesn't drift during contact phases (e.g. a switch push). - move_base=False, - ) + action = self._move_ik_action(phase, pb_state, current_pose, + target_pose, finger_status) except utils.OptionExecutionFailure as e: cur = current_pose.position tgt = target_pose.position @@ -1434,6 +1443,49 @@ def _execute_move_ik(self, phase: Phase, state: State, f"current=({cur[0]:.3f}, {cur[1]:.3f}, {cur[2]:.3f}), " f"target=({tgt[0]:.3f}, {tgt[1]:.3f}, {tgt[2]:.3f}), " f"params={params.tolist()}") from e + if phase.max_step_norm is not None: + finger_idxs = (robot.left_finger_joint_idx, + robot.right_finger_joint_idx) + arm_delta = max( + abs(float(a) - float(c)) for i, ( + a, + c) in enumerate(zip(action.arr, pb_state.joint_positions)) + if i not in finger_idxs) + if arm_delta > self._ik_joint_jump_max: + # IK returned a different branch (e.g. a wrist flip). + # Hold position this step; a persistent flip is caught + # by the stall abort armed in _execute_move. + logging.debug( + "[%s/%s] IK joint jump %.2f rad suppressed; holding.", + self._name, phase.name, arm_delta) + fingers = pb_state.joint_positions[robot.left_finger_joint_idx] + return get_change_fingers_action(robot, + pb_state.joint_positions, + fingers, fingers, + self._config.max_vel_norm) + return action + + def _move_ik_action(self, phase: Phase, pb_state: utils.PyBulletState, + current_pose: Pose, target_pose: Pose, + finger_status: str) -> Action: + """One incremental-IK step toward the phase target.""" + robot = self._config.robot + return get_move_end_effector_to_pose_action( + robot=robot, + current_joint_positions=pb_state.joint_positions, + current_pose=current_pose, + target_pose=target_pose, + finger_status=finger_status, + max_vel_norm=(phase.max_step_norm if phase.max_step_norm + is not None else self._config.max_vel_norm), + finger_action_nudge_magnitude=( + self._config.finger_action_nudge_magnitude), + validate=self._config.ik_validate, + # Base positioning is handled once per option by + # _maybe_drive_base; keep incremental IK arm-only so the base + # doesn't drift during contact phases (e.g. a switch push). + move_base=False, + ) def _execute_fingers(self, phase: Phase, state: State, memory: Dict, objects: Sequence[Object], params: Array) -> Action: diff --git a/predicators/ground_truth_models/skill_factories/move_to.py b/predicators/ground_truth_models/skill_factories/move_to.py index 2eaabc56f..128b3256d 100644 --- a/predicators/ground_truth_models/skill_factories/move_to.py +++ b/predicators/ground_truth_models/skill_factories/move_to.py @@ -27,7 +27,7 @@ def _get_home_pose(state, objects, params, config): ) """ -from typing import Optional, Sequence, Tuple +from typing import Callable, Optional, Sequence, Tuple import pybullet as p from gym.spaces import Box @@ -144,6 +144,9 @@ def make_move_to_phase( validate_ik: bool = False, check_release_clearance: bool = False, use_motion_planning: Optional[bool] = None, + terminal_fn: Optional[Callable[ + [State, Sequence[Object], Array, SkillConfig], bool]] = None, + max_step_norm: Optional[float] = None, ) -> Phase: """Create a MOVE_TO_POSE phase for use in a ``PhaseSkill``. @@ -165,6 +168,13 @@ def make_move_to_phase( collision-free planner asked for such a goal either fails or reaches it by a detour that arrives from the wrong direction (see ``create_push_skill``). + terminal_fn: Optional custom terminal override forwarded to the + ``Phase`` (e.g. "held object made contact"); when ``None`` + the phase uses the default distance-based terminal. + max_step_norm: Optional gentle-stroke step clamp forwarded to + the ``Phase`` (see ``Phase.max_step_norm``): small EE steps + plus a joint-jump guard and the stall abort, for + incremental-IK phases that deliberately seek contact. Returns: A ``Phase`` that can be included in a ``PhaseSkill``. @@ -221,9 +231,11 @@ def _target_fn( name=name, action_type=PhaseAction.MOVE_TO_POSE, target_fn=_target_fn, + terminal_fn=terminal_fn, expect_contact=expect_contact, allow_shallow_held_object_contacts=allow_shallow_held_object_contacts, validate_ik=validate_ik, check_release_clearance=check_release_clearance, use_motion_planning=plan_motion, + max_step_norm=max_step_norm, ) diff --git a/predicators/ground_truth_models/skill_factories/place.py b/predicators/ground_truth_models/skill_factories/place.py index 2cc036a25..fc3976d59 100644 --- a/predicators/ground_truth_models/skill_factories/place.py +++ b/predicators/ground_truth_models/skill_factories/place.py @@ -29,9 +29,10 @@ ) """ -from typing import Optional, Sequence, Tuple +from typing import Optional, Sequence, Set, Tuple import numpy as np +import pybullet as p from predicators.ground_truth_models.skill_factories.base import \ _RELEASE_CLEAR_SLACK, _RELEASE_OPEN_STEP, Phase, PhaseAction, PhaseSkill, \ @@ -40,6 +41,64 @@ make_move_to_phase from predicators.structs import Array, Object, ParameterizedOption, State, Type +# Contact distances below this count as touching for the guarded +# settle (getContactPoints reports near-contacts up to the contact +# processing threshold with small positive separations). +_SETTLE_CONTACT_DIST = 1e-4 + + +def _held_assembly_in_contact(state: State) -> bool: + """True when the held object, or any body welded to it, touches a body + outside the held assembly (the robot excluded). + + Weld partners matter: a carried welded assembly (e.g. a fused span + row) usually touches down through an OUTER member, not the grasped + one. The assembly is discovered generically by BFS over the client's + fixed constraints, skipping the grasp constraint (any fixed + constraint involving the robot body). + """ + sim_state = getattr(state, "simulator_state", None) + if not isinstance(sim_state, dict): + return False + client = sim_state.get("physics_client_id") + robot_id = sim_state.get("robot_id") + if client is None: + return False + held_id: Optional[int] = None + for obj in state: + if "is_held" in obj.type.feature_names and \ + state.get(obj, "is_held") > 0.5: + held_id = getattr(obj, "id", None) + break + if held_id is None: + return False + edges = [] + for i in range(p.getNumConstraints(physicsClientId=client)): + cid = p.getConstraintUniqueId(i, physicsClientId=client) + info = p.getConstraintInfo(cid, physicsClientId=client) + parent, child, joint_type = info[0], info[2], info[4] + if joint_type != p.JOINT_FIXED or robot_id in (parent, child): + continue + edges.append((parent, child)) + assembly: Set[int] = {held_id} + frontier = [held_id] + while frontier: + cur = frontier.pop() + for a, b in edges: + for nxt in ((b, ) if a == cur else (a, ) if b == cur else ()): + if nxt not in assembly: + assembly.add(nxt) + frontier.append(nxt) + for body in assembly: + for cp in p.getContactPoints(bodyA=body, physicsClientId=client): + other = cp[2] + if other == robot_id or other in assembly: + continue + if cp[8] < _SETTLE_CONTACT_DIST: + return True + return False + + # Canonical continuous parameters for Place. _PLACE_PARAMS = [ ("target_x (world x position for placement)", 0.4, 1.1), @@ -57,6 +116,7 @@ def create_place_skill( param_defs: Optional[Sequence[Tuple[str, float, float]]] = None, compensate_held_offset: bool = False, compensate_held_z: bool = False, + settle_to_contact_depth: Optional[float] = None, ) -> ParameterizedOption: """Create a multi-phase place skill that releases a held object. @@ -112,6 +172,24 @@ def create_place_skill( depth AND the pick's IK z-residual into ``release_z`` -- and a deep grasp (~2 cm residual) drives the held object into the support surface at the descend goal. + settle_to_contact_depth: If set, insert a guarded + **SettleToContact** phase between the descent and the + release: an incremental-IK contact stroke (no BiRRT -- + its goal is intentionally at/inside the support) that + lowers the held object up to this many meters below + ``release_z`` and stops at the FIRST contact of the held + assembly (the held object or anything welded to it) + with a body outside the assembly. The release then + happens at essentially zero gap, eliminating the + free-fall bounce-and-slide scatter of an open-loop drop. + ``release_z`` stays the (collision-checked) descend goal, + so it must remain clear of the scene; choose the depth to + exceed the largest drop clearance a sampler uses. If + nothing is contacted within the depth, the phase ends at + the depth and the place degrades to a normal (lower) + drop. Note the release-clearance check still validates + the finger-opening sweep at ``release_z``, an upper bound + of the actual release pose. Returns: A ``ParameterizedOption`` implementing the place skill. @@ -238,14 +316,69 @@ def _drop_pose( # at transport height, clear of the scene. partial_release = config.release_until_ungrasped + def _settle_pose( + state: State, + objects: Sequence[Object], + params: Array, + cfg: SkillConfig, + ) -> Tuple[float, float, float, float]: + assert settle_to_contact_depth is not None + x, y, z, yaw = _drop_pose(state, objects, params, cfg) + return x, y, z - settle_to_contact_depth, yaw + + def _settled_or_at_depth( + state: State, + objects: Sequence[Object], + params: Array, + cfg: SkillConfig, + ) -> bool: + # Contact ends the stroke; reaching the full depth (nothing + # under the object within the budget) is the fallback so the + # phase always terminates. + if _held_assembly_in_contact(state): + return True + robot_obj = objects[0] + current = (state.get(robot_obj, + "x"), state.get(robot_obj, + "y"), state.get(robot_obj, "z")) + tx, ty, tz, _ = _settle_pose(state, objects, params, cfg) + squared_dist = float( + np.sum(np.square(np.subtract(current, (tx, ty, tz))))) + return squared_dist < cfg.move_to_pose_tol + phases = [] + # A place's first move starts right after a pick, where a shallow + # lift plus grasp-constraint droop can leave the held object modeled + # grazing the surface it was picked from; allow those shallow start + # contacts (the first motion is away from the surface) instead of + # rejecting the whole plan at the start config. if use_move_above: - phases.append(make_move_to_phase("MoveAbove", _above_pose, "closed")) + phases.append( + make_move_to_phase("MoveAbove", + _above_pose, + "closed", + allow_shallow_held_object_contacts=True)) phases.append( - make_move_to_phase("Descend" if use_move_above else "MoveToDrop", - _drop_pose, - "closed", - check_release_clearance=True)) + make_move_to_phase( + "Descend" if use_move_above else "MoveToDrop", + _drop_pose, + "closed", + allow_shallow_held_object_contacts=not use_move_above, + check_release_clearance=True)) + if settle_to_contact_depth is not None: + # Gentle stroke: 3 mm steps bound the post-contact overshoot + # (contact is only observed at the next policy step) and arm + # the joint-jump guard -- single-shot IK once answered a plain + # 2 cm descent with a wrist-flipped branch, and the flipped + # retreat then batted the released block across the table. + phases.append( + make_move_to_phase("SettleToContact", + _settle_pose, + "closed", + expect_contact=True, + use_motion_planning=False, + terminal_fn=_settled_or_at_depth, + max_step_norm=0.003)) if partial_release: phases.extend([ Phase( diff --git a/scripts/configs/predicatorv3/envs/all.yaml b/scripts/configs/predicatorv3/envs/all.yaml index 3407dea95..b17a015d8 100644 --- a/scripts/configs/predicatorv3/envs/all.yaml +++ b/scripts/configs/predicatorv3/envs/all.yaml @@ -418,3 +418,11 @@ ENVS: # in the codebase; path fidelity is worth the ~2x steps per # motion-planned phase (episodes stay far under the horizon). pybullet_birrt_path_subsample_ratio: 1 + # Right after a pick, the planning sim's reconstruction of the + # held object (grasp-transform capture + orientation round-trip) + # occasionally models it up to ~9 mm into the table it was just + # lifted off -- physically impossible for a 2-3 cm lift, but it + # rejects the next Place's start config. Deepen the shallow-held + # allowance past the artifact (start contacts only; the first + # motion is upward). + pybullet_birrt_shallow_held_contact_margin: -0.012 diff --git a/tests/approaches/test_oracle_process_planning_bridge.py b/tests/approaches/test_oracle_process_planning_bridge.py index 4c952f406..141605789 100644 --- a/tests/approaches/test_oracle_process_planning_bridge.py +++ b/tests/approaches/test_oracle_process_planning_bridge.py @@ -49,6 +49,10 @@ def _oracle_bridge_config() -> dict: # default 1 mm margin turns those into unrecoverable BiRRT # start/goal rejections. "pybullet_birrt_contact_margin": -0.005, + # Post-pick reconstruction artifacts can model the held object + # up to ~9 mm into its pick surface at the next phase's start + # config; allow escaping those (start contacts only). + "pybullet_birrt_shallow_held_contact_margin": -0.012, # Each Wait ends on the FIRST atom change, so a plan waiting on # several concurrent cures can need a cheap replan for the tail # (which reduces to "Wait until the remaining joint cures"). diff --git a/tests/envs/test_pybullet_bridge.py b/tests/envs/test_pybullet_bridge.py index c9ea525a6..2dd54a066 100644 --- a/tests/envs/test_pybullet_bridge.py +++ b/tests/envs/test_pybullet_bridge.py @@ -113,6 +113,67 @@ def test_glue_cure_weld_lifecycle(env_and_task): assert len(env._weld_constraints) == 1 +def test_place_settles_to_contact(): + """Place must release at first contact instead of free-falling. + + With a release_z several mm above resting height, the settle phase + lowers the block to the support before opening, so the block lands + at resting height with no bounce spin. Regression coverage for two + failure modes: the old open-loop drop bounced and slid (mm-scale + scatter that flipped this domain's tight tolerances), and an early + settle implementation took the whole stroke in one IK step whose + wrist-flipped branch batted the released block across the table + (~9 cm slide, 0.16 rad spin). + """ + utils.reset_config({ + "env": "pybullet_bridge", + "seed": 0, + "num_train_tasks": 1, + "num_test_tasks": 0, + "skill_phase_use_motion_planning": True, + "pybullet_ik_validate": True, + "pybullet_birrt_contact_margin": -0.005, + "pybullet_birrt_path_subsample_ratio": 1, + }) + from predicators.envs.pybullet_bridge import \ + PyBulletBridgeEnv # pylint: disable=import-outside-toplevel + from predicators.ground_truth_models import \ + get_gt_options # pylint: disable=import-outside-toplevel + env = PyBulletBridgeEnv(use_gui=False) + try: + task = env._generate_train_tasks()[0] + env._set_state(task.init) + state = env._get_state() + options = {o.name: o for o in get_gt_options(env.get_name())} + span1 = next(b for b in env._blocks if b.name == "span1") + + def run_option(opt, objs, params): + nonlocal state + ground = opt.ground(objs, np.array(params, dtype=np.float32)) + assert ground.initiable(state) + for _ in range(200): + env.step(ground.policy(state)) + state = env._get_state() + if ground.terminal(state): + return + raise AssertionError(f"{opt.name} did not terminate") + + run_option(options["PickBlock"], [env._robot, span1], [0.002]) + resting_z = env.table_height + env.span_half_extents[2] + tx, ty = 0.75, 1.25 + run_option(options["Place"], [env._robot], + [tx, ty, resting_z + 0.008, 0.0]) + # Landed at resting height (no residual drop), near the target, + # without spinning. + assert abs(state.get(span1, "z") - resting_z) < 0.002 + assert abs(state.get(span1, "x") - tx) < 0.01 + assert abs(state.get(span1, "y") - ty) < 0.01 + assert abs(state.get(span1, "yaw")) < 0.03 + finally: + import pybullet as p # pylint: disable=import-outside-toplevel + p.disconnect(env._physics_client_id) + + def test_sim_data_isolated_between_env_instances(env_and_task): """Glue/cure/attached written by one env instance must never leak into another env instance through shared State Object instances. From 3e953df5e83daa131e092e20ec12895ed7ca960f Mon Sep 17 00:00:00 2001 From: Yichao Liang Date: Mon, 17 Aug 2026 10:22:05 -0400 Subject: [PATCH 07/30] bridge: sustained-dwell glue wetting; observable block half extents Wetting was instantaneous: the moment the bottle tip crossed the 2 cm apply radius for a single step, the face wet. That let a DRIVE-BY graze glue -- a marginal target whose approach never dwells near the dab could still wet when its retreat clipped the radius for one step, a step-phasing coin flip. The seed-1 agent run died exactly this way: its glue hover validated 6/6 in the sandbox on lucky phasing, then missed by ~2 mm for real, and the run had no recovery path. Wetting now requires the tip inside the radius for wet_streak_steps (3) CONSECUTIVE steps, implemented identically in the env and both GT simulator variants (FO + PO) so the sandbox can never validate a graze that real phasing then misses. The streak rides in the existing glue_* feature as partials (0.2/0.4, kept <= 0.5 so every is-wet reader -- classifiers, cure gate, patch visuals -- still sees a dry face), so it round-trips through _set_state like any other feature and needs no new state schema. This immediately exposed that the oracle itself only spent ~1 step in range (2.4 cm/step approach, immediate retreat), so the phase machinery gains Phase.dwell_steps -- hold at the reached target for N extra policy steps before advancing -- and the bridge MoveTo dwells wet_streak_steps + 1 at its target. Deliberate aims wet reliably; grazes never do; any agent using MoveTo inherits the dwell. Blocks also gain observable body-frame half extents (half_x/y/z, in FO and PO modes): agents needed block dimensions to compute face centers, dab points, and touch spacings, and were burning solve budget probing the physics for them (one attempt spent $12 re-deriving geometry the type now simply reports). Verified: 24-instance demonstrator sweep 22/24 (both failures are known flaky modeling-artifact tails unrelated to wetting; every glue step wet correctly), full suite 1485 passed, new regression tests pin the contract on both sides (env: drive-by graze never wets, streak resets on interruption; GT sim: identical behavior on hand-built states). --- predicators/envs/pybullet_bridge.py | 52 ++++++++++++-- .../bridge/gt_simulator.py | 68 ++++++++++++------- .../bridge/gt_simulator_po.py | 23 +++++-- .../ground_truth_models/bridge/options.py | 14 +++- .../skill_factories/base.py | 30 +++++++- .../skill_factories/move_to.py | 17 ++++- .../test_bridge_gt_simulator.py | 37 ++++++++++ tests/envs/test_pybullet_bridge.py | 60 +++++++++++++++- 8 files changed, 259 insertions(+), 42 deletions(-) diff --git a/predicators/envs/pybullet_bridge.py b/predicators/envs/pybullet_bridge.py index c6a178ad0..acd54f679 100644 --- a/predicators/envs/pybullet_bridge.py +++ b/predicators/envs/pybullet_bridge.py @@ -201,6 +201,18 @@ class PyBulletBridgeEnv(PyBulletEnv): # Only the single nearest in-range face is wetted per step, so # neighboring dab points (>= 2.5 cm apart) don't double-wet. apply_glue_radius: ClassVar[float] = 0.02 + # Consecutive in-range steps required to wet a face. Wetting used + # to be instantaneous, so a one-step drive-by crossing of the + # radius (e.g. a bottle retreat clipping the sphere on its way up) + # could wet a face -- a step-phasing coin flip that let marginal + # glue targets validate in the sandbox and then miss for real. + # Requiring a sustained dwell makes grazes fail deterministically + # everywhere. The streak rides IN the glue_* feature as partials of + # _WET_PARTIAL per step (kept <= 0.5 so every "is wet" reader -- + # classifiers, cure gate, patch visuals -- still sees a dry face), + # so it round-trips through _set_state like any other feature. + wet_streak_steps: ClassVar[int] = 3 + _WET_PARTIAL: ClassVar[float] = 0.2 # Dab points hover this far off the face surface. dab_margin: ClassVar[float] = 0.005 # Stacking tolerances for the top-face cure detector (leg-on-leg). @@ -313,9 +325,13 @@ class PyBulletBridgeEnv(PyBulletEnv): # degenerate, so roll folds to 0 there; reconstruction checks # compare the triple as a geodesic rotation (gimbal-safe), not # axis-by-axis. + # half_x/y/z are the block's BODY-FRAME half extents (constant; + # local x is the long axis). Observable geometry: an agent needs + # them to compute face centers, dab points, and touch spacings + # without probing the physics for block dimensions. _block_features_common = [ - "x", "y", "z", "roll", "pitch", "yaw", "is_held", "glue_top", - "glue_end_a", "glue_end_b" + "x", "y", "z", "roll", "pitch", "yaw", "half_x", "half_y", "half_z", + "is_held", "glue_top", "glue_end_a", "glue_end_b" ] # attached_* (partner block index, -1 = none) are observable ONLY # in FO mode: no real perception system emits "attached to block @@ -765,6 +781,12 @@ def _get_domain_specific_feature(self, obj: Object, feature: str) -> float: return self._attr(obj, feature, 0.0) if feature.startswith("attached_"): return self._attr(obj, feature, -1.0) + if feature == "half_x": + return self.block_half_extents[0] + if feature == "half_y": + return self.block_half_extents[1] + if feature == "half_z": + return self.block_half_extents[2] raise ValueError(f"Unknown feature {feature} for object {obj}.") def _is_block(self, obj: Object) -> bool: @@ -1079,11 +1101,12 @@ def _domain_specific_step(self) -> None: state = self._get_state() blocks = state.get_objects(self._block_type) - # 1. Glue application: wet the single nearest in-range face. + # 1. Glue application: sustained proximity wets the single + # nearest in-range face (see wet_streak_steps). + best: Optional[Tuple[Object, str]] = None if state.get(self._bottle, "is_held") > 0.5: tip = (state.get(self._bottle, "x"), state.get(self._bottle, "y"), state.get(self._bottle, "z") - self.bottle_half_extents[2]) - best: Optional[Tuple[Object, str]] = None best_dist = self.apply_glue_radius for blk in blocks: for face in GLUE_FACES: @@ -1096,9 +1119,18 @@ def _domain_specific_step(self) -> None: if dist < best_dist: best = (blk, face) best_dist = dist - if best is not None: - blk, face = best - self._set_attr(blk, f"glue_{face}", 1.0) + for blk in blocks: + for face in GLUE_FACES: + prev = self._attr(blk, f"glue_{face}", 0.0) + if best == (blk, face): + streak = int(round(prev / self._WET_PARTIAL)) + 1 + self._set_attr( + blk, f"glue_{face}", + 1.0 if streak >= self.wet_streak_steps else streak * + self._WET_PARTIAL) + elif 0.0 < prev <= 0.5: + # Not the in-range face this step: the streak breaks. + self._set_attr(blk, f"glue_{face}", 0.0) # 2. Curing: wet faces in aligned resting contact tick; at the # threshold the joint latches irreversibly and welds. @@ -1516,6 +1548,12 @@ def _make_tasks(self, num_tasks: int, -np.pi / 2 if is_leg else 0.0, "yaw": 0.0, + "half_x": + self.block_half_extents[0], + "half_y": + self.block_half_extents[1], + "half_z": + self.block_half_extents[2], "is_held": 0.0, "r": diff --git a/predicators/ground_truth_models/bridge/gt_simulator.py b/predicators/ground_truth_models/bridge/gt_simulator.py index a4fb51514..957b926b2 100644 --- a/predicators/ground_truth_models/bridge/gt_simulator.py +++ b/predicators/ground_truth_models/bridge/gt_simulator.py @@ -39,6 +39,14 @@ # Physical defaults matching pybullet_bridge.py. CURE_THRESHOLD = 25.0 APPLY_GLUE_RADIUS = 0.02 +# Consecutive in-range steps required to wet a face (must match the +# env). The streak rides in the glue_* feature as partials of +# WET_PARTIAL per step, kept <= 0.5 so every "is wet" reader still +# sees a dry face until the streak completes. A one-step drive-by +# crossing of the radius therefore never wets a face -- gluing takes a +# deliberate dwell at the dab. +WET_STREAK_STEPS = 3 +WET_PARTIAL = 0.2 STACK_ALIGN_TOL = 0.025 LATERAL_PERP_TOL = 0.03 SEAT_X_WINDOW = 0.045 @@ -207,38 +215,48 @@ def _block_index(blocks: List[Object]) -> Dict[str, int]: def _glue_application(state: State, updates: ResidualUpdate, params: Params) -> ResidualUpdate: - """Wet the single nearest face within the bottle tip's radius.""" + """Advance the wet streak of the single nearest in-range face. + + Wet faces (glue > 0.5) stay wet; the nearest in-range dry face gains + WET_PARTIAL of streak per step and latches to 1.0 on the + WET_STREAK_STEPS-th consecutive step; every other partial streak + resets to 0. + """ objs = objs_by_type(state) blocks = objs.get("block", []) bottles = objs.get("bottle", []) - # Carry existing glue by default. - for blk in blocks: - for face in GLUE_FACES: - updates.setdefault(blk, {})[f"glue_{face}"] = float( - state.get(blk, f"glue_{face}")) held = [b for b in bottles if state.get(b, "is_held") > 0.5] - if not held: - return updates - bottle = held[0] - tip = np.array([ - float(state.get(bottle, "x")), - float(state.get(bottle, "y")), - float(state.get(bottle, "z")) - BOTTLE_HALF_H - ]) best, best_d = None, float(params["apply_glue_radius"]) + if held: + bottle = held[0] + tip = np.array([ + float(state.get(bottle, "x")), + float(state.get(bottle, "y")), + float(state.get(bottle, "z")) - BOTTLE_HALF_H + ]) + for blk in blocks: + for face in GLUE_FACES: + if state.get(blk, f"glue_{face}") > 0.5: + continue + if state.get(blk, f"attached_{face}") >= 0: + continue + d = float( + np.linalg.norm(tip - + np.array(_dab_point(state, blk, face)))) + if d < best_d: + best, best_d = (blk, face), d for blk in blocks: for face in GLUE_FACES: - if state.get(blk, f"glue_{face}") > 0.5: - continue - if state.get(blk, f"attached_{face}") >= 0: - continue - d = float( - np.linalg.norm(tip - np.array(_dab_point(state, blk, face)))) - if d < best_d: - best, best_d = (blk, face), d - if best is not None: - blk, face = best - updates.setdefault(blk, {})[f"glue_{face}"] = 1.0 + prev = float(state.get(blk, f"glue_{face}")) + if best == (blk, face): + streak = int(round(prev / WET_PARTIAL)) + 1 + nxt = 1.0 if streak >= WET_STREAK_STEPS \ + else streak * WET_PARTIAL + elif 0.0 < prev <= 0.5: + nxt = 0.0 # streak broken + else: + nxt = prev + updates.setdefault(blk, {})[f"glue_{face}"] = nxt return updates diff --git a/predicators/ground_truth_models/bridge/gt_simulator_po.py b/predicators/ground_truth_models/bridge/gt_simulator_po.py index ff2d6e272..b516b5765 100644 --- a/predicators/ground_truth_models/bridge/gt_simulator_po.py +++ b/predicators/ground_truth_models/bridge/gt_simulator_po.py @@ -39,6 +39,11 @@ CURE_THRESHOLD = 25.0 APPLY_GLUE_RADIUS = 0.02 +# Consecutive in-range steps to wet a face; the streak rides in the +# glue_* observable as WET_PARTIAL per step (<= 0.5 = still dry). A +# one-step drive-by crossing of the radius never wets a face. +WET_STREAK_STEPS = 3 +WET_PARTIAL = 0.2 STACK_ALIGN_TOL = 0.025 LATERAL_PERP_TOL = 0.03 SEAT_X_WINDOW = 0.045 @@ -187,15 +192,18 @@ def _gluing(observation: State, latent: Dict[str, Any], history: History, for face in GLUE_FACES } - # 1. Glue application: nearest unattached dry face within radius. + # 1. Glue application: sustained proximity wets the nearest + # unattached dry face (streak rides in the glue_* observable as + # WET_PARTIAL steps, matching the env; see gt_simulator). held = [b for b in bottles if observation.get(b, "is_held") > 0.5] + best = None if held: tip = np.array([ float(observation.get(held[0], "x")), float(observation.get(held[0], "y")), float(observation.get(held[0], "z")) - BOTTLE_HALF_H ]) - best, best_d = None, float(params["apply_glue_radius"]) + best_d = float(params["apply_glue_radius"]) for blk in blocks: for face in GLUE_FACES: if glue_next[blk][face] > 0.5 or \ @@ -206,8 +214,15 @@ def _gluing(observation: State, latent: Dict[str, Any], history: History, tip - np.array(_dab_point(observation, blk, face)))) if d < best_d: best, best_d = (blk, face), d - if best is not None: - glue_next[best[0]][best[1]] = 1.0 + for blk in blocks: + for face in GLUE_FACES: + prev = glue_next[blk][face] + if best == (blk, face): + streak = int(round(prev / WET_PARTIAL)) + 1 + glue_next[blk][face] = 1.0 \ + if streak >= WET_STREAK_STEPS else streak * WET_PARTIAL + elif 0.0 < prev <= 0.5: + glue_next[blk][face] = 0.0 # streak broken # 2. Curing: hidden counters keyed by the wet face; the latch # writes the latent attachment relation, never a feature. diff --git a/predicators/ground_truth_models/bridge/options.py b/predicators/ground_truth_models/bridge/options.py index c73f0a26c..e83d8d904 100644 --- a/predicators/ground_truth_models/bridge/options.py +++ b/predicators/ground_truth_models/bridge/options.py @@ -50,7 +50,11 @@ # EE-to-held offset, so the sampled target is exactly where the held # object goes regardless of grasp depth or the pick's IK residual. The # glue samplers use this to land the held bottle's tip on a face dab -# point (tip = center minus the bottle half-height). +# point (tip = center minus the bottle half-height). Wetting requires +# the tip within apply_glue_radius of the dab for wet_streak_steps +# CONSECUTIVE steps -- the skill dwells at the reached target to +# provide them, so glue targets must put the tip AT the dab; a +# trajectory that merely passes through the radius never wets. # # The x bounds extend one span half-length past the block workspace: a # block STAGED near the workspace edge has its end-face dab point up to @@ -239,6 +243,14 @@ def _move_to_pose( retreat=True, validate_ik=True, base_mode="home", + # Hold at the reached target before retreating: wetting a + # glue face needs the tip inside the apply radius for + # wet_streak_steps CONSECUTIVE steps (a drive-by crossing + # never wets, by design), and without a dwell the approach + # spends as little as one step in range before the retreat + # exits the radius. +1 covers the approach/retreat edge + # steps. + dwell_steps=cls.env_cls.wet_streak_steps + 1, ) return { diff --git a/predicators/ground_truth_models/skill_factories/base.py b/predicators/ground_truth_models/skill_factories/base.py index e162cd746..7ce4aae34 100644 --- a/predicators/ground_truth_models/skill_factories/base.py +++ b/predicators/ground_truth_models/skill_factories/base.py @@ -274,6 +274,7 @@ def _fmt_option_params(params: Array) -> str: _RELEASE_CLEAR_SLACK = 0.008 _RELEASE_CHECK_BUFFER = _RELEASE_OPEN_STEP + _RELEASE_CLEAR_SLACK + 0.002 _IK_STALL_BEST_KEY = "ik_stall_best_{}" # best EE-to-target distance seen +_DWELL_COUNT_KEY = "dwell_count_{}" # post-terminal hold steps taken _IK_STALL_COUNT_KEY = "ik_stall_count_{}" # steps since last improvement @@ -338,6 +339,16 @@ class Phase: # open, the target moves further out, and the phase never terminates # short of fully open. anchor_finger_target: bool = False + # Hold at the reached target for this many extra policy steps after + # the phase's terminal condition first holds, before advancing to + # the next phase (the policy keeps commanding the same target, + # which is a hold). Use for "move there and DWELL" semantics -- + # e.g. a glue application that requires sustained tip proximity + # rather than a drive-by crossing. Only delays PHASE advancement: + # a dwell on the FINAL phase does not delay the option's overall + # terminal (the counter lives in the policy, which stops running + # once the option is terminal). + dwell_steps: int = 0 # Gentle-stroke mode for incremental-IK phases that deliberately seek # contact (e.g. a place's settle-to-contact). When set, this overrides # the EE step clamp (meters per step; default config.max_vel_norm), and @@ -421,8 +432,20 @@ def _policy(self, state: State, memory: Dict, objects: Sequence[Object], phase_idx = memory["phase_idx"] phase = self._phases[phase_idx] - # Check if current phase is terminal → advance. + # Check if current phase is terminal → advance. A phase with + # dwell_steps holds at its reached target for that many extra + # policy steps before advancing (the policy keeps commanding the + # same phase target, which is a hold). The counter lives here, + # in the once-per-step policy, NOT in _phase_is_terminal -- + # terminal checks can run several times per step (policy + + # monitors) and would over-count. if self._phase_is_terminal(phase, state, memory, objects, params): + dwell_key = _DWELL_COUNT_KEY.format(id(phase)) + dwelled = memory.get(dwell_key, 0) + if dwelled < phase.dwell_steps: + memory[dwell_key] = dwelled + 1 + return self._execute_phase(phase, state, memory, objects, + params) phase_idx += 1 memory["phase_idx"] = phase_idx if phase_idx >= len(self._phases): @@ -433,6 +456,11 @@ def _policy(self, state: State, memory: Dict, objects: Sequence[Object], logging.debug("[%s] Advanced to phase %d: %s", self._name, phase_idx, phase.name) + return self._execute_phase(phase, state, memory, objects, params) + + def _execute_phase(self, phase: Phase, state: State, memory: Dict, + objects: Sequence[Object], params: Array) -> Action: + """Dispatch one policy step of ``phase`` by its action type.""" if phase.action_type == PhaseAction.MOVE_TO_POSE: return self._execute_move(phase, state, memory, objects, params) assert phase.action_type == PhaseAction.CHANGE_FINGERS diff --git a/predicators/ground_truth_models/skill_factories/move_to.py b/predicators/ground_truth_models/skill_factories/move_to.py index 128b3256d..10a7260fe 100644 --- a/predicators/ground_truth_models/skill_factories/move_to.py +++ b/predicators/ground_truth_models/skill_factories/move_to.py @@ -50,6 +50,7 @@ def create_move_to_skill( retreat: bool = False, validate_ik: bool = False, base_mode: Optional[str] = None, + dwell_steps: int = 0, ) -> ParameterizedOption: """Create a move-to-pose skill. @@ -79,6 +80,10 @@ def create_move_to_skill( retreat: Append the transport-height retreat phase. validate_ik: Gate the Move phase's target through validated IK. base_mode: Optional ``PhaseSkill`` base mode (e.g. ``"home"``). + dwell_steps: Hold at the reached target for this many extra + steps before retreating (see ``Phase.dwell_steps``) -- + "move there and DWELL" semantics, e.g. for sustained- + proximity glue application. Returns: A ``ParameterizedOption`` implementing the move-to-pose skill. @@ -97,8 +102,10 @@ def _above_pose( if use_move_above: phases.append(make_move_to_phase("MoveAbove", _above_pose)) phases.append( - make_move_to_phase("Move", get_target_pose_fn, - validate_ik=validate_ik)) + make_move_to_phase("Move", + get_target_pose_fn, + validate_ik=validate_ik, + dwell_steps=dwell_steps)) if retreat: phases.append( make_move_to_phase("Retreat", @@ -147,6 +154,7 @@ def make_move_to_phase( terminal_fn: Optional[Callable[ [State, Sequence[Object], Array, SkillConfig], bool]] = None, max_step_norm: Optional[float] = None, + dwell_steps: int = 0, ) -> Phase: """Create a MOVE_TO_POSE phase for use in a ``PhaseSkill``. @@ -175,6 +183,10 @@ def make_move_to_phase( the ``Phase`` (see ``Phase.max_step_norm``): small EE steps plus a joint-jump guard and the stall abort, for incremental-IK phases that deliberately seek contact. + dwell_steps: Hold at the reached target for this many extra + policy steps before advancing (see ``Phase.dwell_steps``), + for "move there and DWELL" semantics such as sustained- + proximity glue application. Returns: A ``Phase`` that can be included in a ``PhaseSkill``. @@ -238,4 +250,5 @@ def _target_fn( check_release_clearance=check_release_clearance, use_motion_planning=plan_motion, max_step_norm=max_step_norm, + dwell_steps=dwell_steps, ) diff --git a/tests/code_sim_learning/test_bridge_gt_simulator.py b/tests/code_sim_learning/test_bridge_gt_simulator.py index 4d34fa9c1..58ebeb0a5 100644 --- a/tests/code_sim_learning/test_bridge_gt_simulator.py +++ b/tests/code_sim_learning/test_bridge_gt_simulator.py @@ -68,6 +68,43 @@ def _roll_until_latched(state, rules, params, blk, slot, max_steps): return None, state +_BOTTLE_TYPE = Type("bottle", ["x", "y", "z", "rot", "is_held"]) + + +def test_sustained_wetting_matches_env(): + """The sim's glue rule requires the same sustained dwell as the env: a tip + parked at a dab wets on the WET_STREAK_STEPS-th consecutive step, and an + interrupted streak resets -- so a drive-by crossing of the apply radius can + never wet a face in the sandbox either.""" + from predicators.ground_truth_models.bridge.gt_simulator import \ + WET_STREAK_STEPS # pylint: disable=import-outside-toplevel + rules, params = _bridge_sim() + span0, span0_feats = _make_block("span0", 0.6, 1.2, + _TABLE_Z + _SPAN_HALF[2]) + bottle = Object("bottle", _BOTTLE_TYPE) + # Held bottle with its tip at span0's end_b dab (above the face's + # top edge: z + half_z + dab margin, tip = center - half height). + dab_z = _TABLE_Z + 2 * _SPAN_HALF[2] + 0.005 + bottle_feats = np.array([0.6 + _SPAN_HALF[0], 1.2, dab_z + 0.03, 0.0, 1.0], + dtype=np.float32) + state = State({span0: span0_feats, bottle: bottle_feats}) + + for step in range(WET_STREAK_STEPS): + wet = state.get(span0, "glue_end_b") + assert wet <= 0.5, f"wet after only {step} steps" + state = merge_updates(state, apply_rules(state, rules, params)) + assert state.get(span0, "glue_end_b") > 0.5 + + # Interrupted streak: two in-range steps, one out-of-range, resets. + state = State({span0: span0_feats.copy(), bottle: bottle_feats.copy()}) + for _ in range(WET_STREAK_STEPS - 1): + state = merge_updates(state, apply_rules(state, rules, params)) + assert 0.0 < state.get(span0, "glue_end_b") <= 0.5 + state.set(bottle, "z", dab_z + 0.2) + state = merge_updates(state, apply_rules(state, rules, params)) + assert state.get(span0, "glue_end_b") == 0.0 + + def test_bridge_gt_simulator_loads(): """The factory registry resolves pybullet_bridge to the FO simulator.""" rules, params = _bridge_sim() diff --git a/tests/envs/test_pybullet_bridge.py b/tests/envs/test_pybullet_bridge.py index 2dd54a066..ec6a66c7e 100644 --- a/tests/envs/test_pybullet_bridge.py +++ b/tests/envs/test_pybullet_bridge.py @@ -60,7 +60,15 @@ def test_glue_cure_weld_lifecycle(env_and_task): s.set(env._robot, "z", dab[2] + 2 * env.bottle_half_extents[2] + 0.005) s.set(env._robot, "fingers", env.closed_fingers) env._set_state(s) + # Wetting takes a SUSTAINED dwell: one in-range step only advances + # the streak (partial <= 0.5 reads as dry), the wet_streak_steps-th + # consecutive step latches the face wet. A one-step drive-by can + # never glue. env.step(_hold_action(env)) + partial = env._get_state().get(leg0, "glue_end_b") + assert 0.0 < partial <= 0.5 + for _ in range(env.wet_streak_steps - 1): + env.step(_hold_action(env)) s2 = env._get_state() assert s2.get(leg0, "glue_end_b") > 0.5 @@ -113,6 +121,54 @@ def test_glue_cure_weld_lifecycle(env_and_task): assert len(env._weld_constraints) == 1 +def test_drive_by_graze_never_wets(env_and_task): + """An interrupted dwell must NOT wet a face: the wet streak resets the + moment the tip leaves the radius. + + Regression: wetting used to be instantaneous, so a one-step + crossing of the apply radius (e.g. a bottle retreat clipping the + sphere on its way up) could wet a face -- a step-phasing coin flip + that let marginal glue targets validate in the sandbox and then + miss in the real rollout. + """ + env, task = env_and_task + env._set_state(task.init) + state = env._get_state() + leg0 = next(b for b in state.get_objects(env._block_type) + if b.name == "leg0") + + def _tip_at_dab(s): + dab = env._face_dab_point(s, leg0, "end_b") + s.set(env._bottle, "x", dab[0]) + s.set(env._bottle, "y", dab[1]) + s.set(env._bottle, "z", dab[2] + env.bottle_half_extents[2]) + s.set(env._bottle, "is_held", 1.0) + s.set(env._robot, "x", dab[0]) + s.set(env._robot, "y", dab[1]) + s.set(env._robot, "z", dab[2] + 2 * env.bottle_half_extents[2] + 0.005) + s.set(env._robot, "fingers", env.closed_fingers) + return s + + # Two in-range steps: a partial streak, still dry. + env._set_state(_tip_at_dab(state.copy())) + env.step(_hold_action(env)) + env.step(_hold_action(env)) + s = env._get_state() + assert 0.0 < s.get(leg0, "glue_end_b") <= 0.5 + # Leave the radius for one step: the streak resets to zero. + s.set(env._bottle, "z", s.get(env._bottle, "z") + 0.1) + s.set(env._robot, "z", s.get(env._robot, "z") + 0.1) + env._set_state(s) + env.step(_hold_action(env)) + s = env._get_state() + assert s.get(leg0, "glue_end_b") == 0.0 + # A fresh sustained dwell still wets. + env._set_state(_tip_at_dab(s)) + for _ in range(env.wet_streak_steps): + env.step(_hold_action(env)) + assert env._get_state().get(leg0, "glue_end_b") > 0.5 + + def test_place_settles_to_contact(): """Place must release at first contact instead of free-falling. @@ -122,8 +178,8 @@ def test_place_settles_to_contact(): failure modes: the old open-loop drop bounced and slid (mm-scale scatter that flipped this domain's tight tolerances), and an early settle implementation took the whole stroke in one IK step whose - wrist-flipped branch batted the released block across the table - (~9 cm slide, 0.16 rad spin). + wrist-flipped branch batted the released block across the table (~9 + cm slide, 0.16 rad spin). """ utils.reset_config({ "env": "pybullet_bridge", From c3810b7d691f72ee9ee6c327a1e53b40c4ad8d05 Mon Sep 17 00:00:00 2001 From: Yichao Liang Date: Mon, 17 Aug 2026 11:06:29 -0400 Subject: [PATCH 08/30] skills: gentle-stroke give-up advance; unbounded support-escape margins Two rails that turn rare hard aborts into soft, replannable outcomes. Gentle strokes (Phase.max_step_norm, e.g. Place's settle-to-contact): when the EE makes no progress toward the stroke target for 8 consecutive steps -- pinned by the joint-jump guard, blocked by a contact on the ROBOT itself (which the held-assembly contact terminal cannot see; a settle once stalled 25 steps this way and the stall abort killed the episode), or saturated at a joint limit -- the stroke now gives up and ADVANCES to the next phase instead of aborting the option. Gentle strokes are best-effort contact seeks below an already-validated pose, so releasing from wherever the stroke reached is strictly better than losing the episode; only a final-phase stroke keeps the stall abort (it has no next phase). This subsumes the previous pin-advance counter. Shallow held start contacts become per-body ESCAPE margins: a normal shallow body keeps the configured shallow margin, while bodies named in the new run_motion_planning unbounded_shallow_bodies (the skill layer passes the sim's static support / table ids) allow whatever depth the START config shows, minus 3 mm of slack -- the start is escapable at any modeled depth, but the path can never go deeper than it began. Rationale: a lift-off phase legitimately begins with the held assembly resting on its support, and planning-time modeling artifacts can show that resting contact tens of mm deep (a welded row's outer span was modeled 20.9 mm into the table it sat on, one mm past the old fixed cap -- an unwinnable margin race), while deep start penetration into a MOVABLE body still signals genuine trouble and keeps the cap. The bridge config's shallow margin stays -0.02 for the movable-body case. Verified: the two targeted failure modes no longer occur (the settle stall now releases and replans; the row-pick start rejection is gone); 24-instance demonstrator sweep holds its ~21-23/24 plateau with the experiment-critical instances (train0 + test0, all seeds) at 8/8 in every sweep; full suite 1485 passed. --- .../skill_factories/base.py | 137 +++++++++++++----- .../pybullet_helpers/motion_planning.py | 37 ++++- scripts/configs/predicatorv3/envs/all.yaml | 17 ++- .../test_oracle_process_planning_bridge.py | 6 +- 4 files changed, 147 insertions(+), 50 deletions(-) diff --git a/predicators/ground_truth_models/skill_factories/base.py b/predicators/ground_truth_models/skill_factories/base.py index 7ce4aae34..9277f2237 100644 --- a/predicators/ground_truth_models/skill_factories/base.py +++ b/predicators/ground_truth_models/skill_factories/base.py @@ -7,7 +7,7 @@ from dataclasses import dataclass, field from enum import Enum, auto from typing import TYPE_CHECKING, Any, Callable, ClassVar, Dict, List, \ - Optional, Sequence, Tuple, cast + Optional, Sequence, Set, Tuple, cast if TYPE_CHECKING: from predicators.envs.pybullet_env import PyBulletEnv @@ -275,6 +275,8 @@ def _fmt_option_params(params: Array) -> str: _RELEASE_CHECK_BUFFER = _RELEASE_OPEN_STEP + _RELEASE_CLEAR_SLACK + 0.002 _IK_STALL_BEST_KEY = "ik_stall_best_{}" # best EE-to-target distance seen _DWELL_COUNT_KEY = "dwell_count_{}" # post-terminal hold steps taken +_STROKE_BEST_KEY = "stroke_best_{}" # gentle stroke: best EE distance +_STROKE_NOPROG_KEY = "stroke_noprog_{}" # gentle stroke: no-progress steps _IK_STALL_COUNT_KEY = "ik_stall_count_{}" # steps since last improvement @@ -352,15 +354,19 @@ class Phase: # Gentle-stroke mode for incremental-IK phases that deliberately seek # contact (e.g. a place's settle-to-contact). When set, this overrides # the EE step clamp (meters per step; default config.max_vel_norm), and - # additionally arms two safety rails in _execute_move_ik/_execute_move: + # additionally arms the rails in _execute_gentle_stroke: # - a joint-jump guard: a mm-scale EE step never legitimately needs a # multi-radian joint move, but single-shot IK (the panda path) can # return a wrist-flipped branch near contact; executing that action # drags the held object through the scene, so the step is replaced # by a hold-position action instead; - # - the incremental-IK stall abort (see _check_ik_stall), so a stroke - # pinned by the guard or blocked short of its target fails the - # option cleanly rather than pressing forever. + # - give-up advance: after a run of no-progress steps (pinned by + # the guard, blocked by a robot-side contact, or saturated at a + # joint limit) the stroke gives up and advances to the next + # phase (best-effort semantics: continuing from wherever it + # reached beats aborting the option); + # - a final-phase stroke keeps the incremental-IK stall abort + # (see _check_ik_stall) as its escape instead. max_step_norm: Optional[float] = None @@ -618,12 +624,81 @@ def _execute_move(self, phase: Phase, state: State, memory: Dict, return self._execute_move_birrt(phase, state, memory, objects, params) if phase.max_step_norm is not None: - # Gentle strokes get the stall abort: a stroke pinned by the - # joint-jump guard (or blocked short of its target) must fail - # the option instead of holding position forever. - self._check_ik_stall(phase, state, memory, objects, params) + return self._execute_gentle_stroke(phase, state, memory, objects, + params) return self._execute_move_ik(phase, state, objects, params) + def _execute_gentle_stroke(self, phase: Phase, state: State, memory: Dict, + objects: Sequence[Object], + params: Array) -> Action: + """One step of a gentle stroke (Phase.max_step_norm) with its rails. + + The joint-jump guard never executes an IK branch flip (a mm- + scale EE step answered with a multi-radian joint move -- + executing one once wrist-flipped the arm and batted a released + block across the table): the step is replaced by a hold. + + Give-up advance: when the EE makes no progress toward the + stroke target for ``_gentle_stroke_giveup_steps`` consecutive + steps -- pinned by the guard, blocked by a contact on the ROBOT + itself (which a held-assembly contact terminal cannot see; a + settle once stalled 25 steps this way and aborted the option), + or saturated at a joint limit -- the stroke gives up and + ADVANCES to the next phase. Gentle strokes are best-effort + contact seeks below an already-validated pose, so continuing + (e.g. releasing) from wherever the stroke reached is strictly + better than aborting the option. A final-phase stroke has no + next phase to advance to, so it keeps the incremental-IK stall + abort as its escape instead. + """ + phase_idx = memory["phase_idx"] + if phase_idx >= len(self._phases) - 1: + self._check_ik_stall(phase, state, memory, objects, params) + else: + current_pose, target_pose, _ = phase.target_fn( + state, objects, params, self._config) + dist = float( + np.linalg.norm( + np.subtract(current_pose.position, target_pose.position))) + best_key = _STROKE_BEST_KEY.format(id(phase)) + count_key = _STROKE_NOPROG_KEY.format(id(phase)) + best = memory.get(best_key) + if best is None or dist < best - self._ik_stall_min_progress: + memory[best_key] = dist + memory[count_key] = 0 + else: + memory[count_key] = memory.get(count_key, 0) + 1 + if memory[count_key] >= self._gentle_stroke_giveup_steps: + memory["phase_idx"] = phase_idx + 1 + nxt = self._phases[phase_idx + 1] + logging.debug( + "[%s/%s] stroke made no progress for %d steps " + "(%.3f m short of the target); advancing to " + "phase %d: %s", self._name, phase.name, + self._gentle_stroke_giveup_steps, dist, phase_idx + 1, + nxt.name) + return self._execute_phase(nxt, state, memory, objects, + params) + action = self._execute_move_ik(phase, state, objects, params) + pb_state = cast(utils.PyBulletState, state) + robot = self._config.robot + finger_idxs = (robot.left_finger_joint_idx, + robot.right_finger_joint_idx) + arm_delta = max( + abs(float(a) - float(c)) + for i, (a, + c) in enumerate(zip(action.arr, pb_state.joint_positions)) + if i not in finger_idxs) + if arm_delta > self._ik_joint_jump_max: + logging.debug( + "[%s/%s] IK joint jump %.2f rad suppressed; " + "holding.", self._name, phase.name, arm_delta) + fingers = pb_state.joint_positions[robot.left_finger_joint_idx] + return get_change_fingers_action(robot, pb_state.joint_positions, + fingers, fingers, + self._config.max_vel_norm) + return action + # Mobile-base positioning. Before the first reach of an option, drive the # (kinematic) base to park `base_standoff` in front of the reach target with # its x aligned to the target x (base y clamped to base_y_max to stay clear @@ -973,6 +1048,15 @@ def _plan_without_simulator( physics_client_id=robot.physics_client_id, ) + @staticmethod + def _sim_table_ids(sim: Any) -> Set[int]: + """The sim env's static support (table) body ids, if any.""" + if hasattr(sim, '_table_ids'): + return set(sim._table_ids) # pylint: disable=protected-access + if hasattr(sim, '_table') and sim._table.id is not None: # pylint: disable=protected-access + return {sim._table.id} # pylint: disable=protected-access + return set() + def _sim_collision_context( self, pb_state: utils.PyBulletState ) -> Tuple[utils.PyBulletState, set, Dict[int, str], Optional[int], Dict[ @@ -1080,11 +1164,7 @@ def _sim_collision_context( world_to_obj[1]) # 4b. Add tables if present. - if hasattr(sim, '_table_ids'): - for tid in sim._table_ids: # pylint: disable=protected-access - collision_bodies.add(tid) - elif hasattr(sim, '_table') and sim._table.id is not None: # pylint: disable=protected-access - collision_bodies.add(sim._table.id) # pylint: disable=protected-access + collision_bodies.update(self._sim_table_ids(sim)) # 4c. Add extra sim collision bodies (e.g. virtual buffer zones). collision_bodies.update(self._config.sim_extra_collision_bodies) @@ -1223,6 +1303,7 @@ def _plan_with_simulator( allow_shallow_held_object_contacts=( phase.allow_shallow_held_object_contacts if phase is not None else False), + unbounded_shallow_bodies=self._sim_table_ids(sim), goal_finger_joint=goal_finger_joint, held_bystander_clearance=self._config.held_bystander_clearance, ) @@ -1259,6 +1340,7 @@ def _plan_with_simulator( allow_shallow_held_object_contacts=( phase.allow_shallow_held_object_contacts if phase is not None else False), + unbounded_shallow_bodies=self._sim_table_ids(sim), goal_finger_joint=goal_finger_joint, held_bystander_clearance=( self._config.held_bystander_clearance), @@ -1451,6 +1533,10 @@ def _check(joints: JointPositions, label: str) -> None: # Gentle strokes (Phase.max_step_norm): any single arm joint asked to # move further than this in one step is a branch flip, not tracking. _ik_joint_jump_max: ClassVar[float] = 0.5 # radians + # Consecutive no-progress steps before a non-final gentle stroke + # gives up and advances to the next phase (see + # _execute_gentle_stroke). + _gentle_stroke_giveup_steps: ClassVar[int] = 8 def _execute_move_ik(self, phase: Phase, state: State, objects: Sequence[Object], params: Array) -> Action: @@ -1471,26 +1557,9 @@ def _execute_move_ik(self, phase: Phase, state: State, f"current=({cur[0]:.3f}, {cur[1]:.3f}, {cur[2]:.3f}), " f"target=({tgt[0]:.3f}, {tgt[1]:.3f}, {tgt[2]:.3f}), " f"params={params.tolist()}") from e - if phase.max_step_norm is not None: - finger_idxs = (robot.left_finger_joint_idx, - robot.right_finger_joint_idx) - arm_delta = max( - abs(float(a) - float(c)) for i, ( - a, - c) in enumerate(zip(action.arr, pb_state.joint_positions)) - if i not in finger_idxs) - if arm_delta > self._ik_joint_jump_max: - # IK returned a different branch (e.g. a wrist flip). - # Hold position this step; a persistent flip is caught - # by the stall abort armed in _execute_move. - logging.debug( - "[%s/%s] IK joint jump %.2f rad suppressed; holding.", - self._name, phase.name, arm_delta) - fingers = pb_state.joint_positions[robot.left_finger_joint_idx] - return get_change_fingers_action(robot, - pb_state.joint_positions, - fingers, fingers, - self._config.max_vel_norm) + # NOTE: gentle strokes (Phase.max_step_norm) reach this through + # _execute_gentle_stroke, which layers the joint-jump guard and + # pin-advance on top of this pure IK step. return action def _move_ik_action(self, phase: Phase, pb_state: utils.PyBulletState, diff --git a/predicators/pybullet_helpers/motion_planning.py b/predicators/pybullet_helpers/motion_planning.py index fb7a8caa1..da7fc55ee 100644 --- a/predicators/pybullet_helpers/motion_planning.py +++ b/predicators/pybullet_helpers/motion_planning.py @@ -27,6 +27,7 @@ def run_motion_planning( base_link_to_held_obj: Optional[NDArray] = None, held_attachments: Optional[Dict[int, Any]] = None, allow_shallow_held_object_contacts: bool = False, + unbounded_shallow_bodies: Optional[Collection[int]] = None, goal_finger_joint: Optional[float] = None, held_bystander_clearance: Optional[float] = None, ) -> Optional[Sequence[JointPositions]]: @@ -56,6 +57,17 @@ def run_motion_planning( ``CFG.pybullet_birrt_held_bystander_clearance`` (the wider berth the held object keeps from bodies the path never intends to approach). + ``unbounded_shallow_bodies`` (used with + ``allow_shallow_held_object_contacts``): bodies -- static supports + like tables -- whose START-state contacts with the held assembly + are allowed at ANY depth, not just down to the shallow margin. A + lift-off phase legitimately begins with the held assembly resting + on its support, and planning-time modeling artifacts can show that + resting contact tens of mm deep (a welded row's outer span was + once modeled 21 mm into the table it sat on); escaping away from a + static support is always safe, whereas deep start penetration into + a movable body still signals genuine trouble and keeps the margin. + Note that this function changes the state of the robot. """ rng = np.random.default_rng(seed) @@ -109,7 +121,14 @@ def _set_state(pt: JointPositions) -> None: shallow_margin = CFG.pybullet_birrt_shallow_held_contact_margin bystander_clearance = CFG.pybullet_birrt_bystander_clearance - allowed_shallow_held_collision_bodies = set() + # Body id -> the penetration depth the held assembly may keep + # against it along the path (an escape allowance for contacts the + # start config already has). Normal shallow-contact bodies get the + # shallow margin; unbounded bodies (static supports) get whatever + # depth the start shows, minus a little slack -- the start is + # escapable at any modeled depth, but the path can never go DEEPER + # than it began. + allowed_shallow_held_margins: Dict[int, float] = {} if allow_shallow_held_object_contacts and held_assembly: _set_state(initial_positions) p.performCollisionDetection(physicsClientId=physics_client_id) @@ -120,8 +139,15 @@ def _set_state(pt: JointPositions) -> None: assembly_body, body, physicsClientId=physics_client_id) penetrating.extend(c[8] for c in contacts if c[8] < hard_margin) - if penetrating and min(penetrating) >= shallow_margin: - allowed_shallow_held_collision_bodies.add(body) + if not penetrating: + continue + start_depth = min(penetrating) + if unbounded_shallow_bodies is not None and \ + body in unbounded_shallow_bodies: + allowed_shallow_held_margins[body] = min( + shallow_margin, start_depth - 0.003) + elif start_depth >= shallow_margin: + allowed_shallow_held_margins[body] = shallow_margin # Bodies the robot or held object starts or deliberately ends within # the clearance of are intended contact partners (support surfaces, @@ -226,8 +252,9 @@ def _collision_fn(pt: JointPositions) -> bool: assembly_body, body, physicsClientId=physics_client_id) contact_distances = [c[8] for c in contacts] - if body in allowed_shallow_held_collision_bodies: - if any(d < shallow_margin for d in contact_distances): + escape_margin = allowed_shallow_held_margins.get(body) + if escape_margin is not None: + if any(d < escape_margin for d in contact_distances): return True continue if any(d < held_margin for d in contact_distances): diff --git a/scripts/configs/predicatorv3/envs/all.yaml b/scripts/configs/predicatorv3/envs/all.yaml index b17a015d8..be853f3d9 100644 --- a/scripts/configs/predicatorv3/envs/all.yaml +++ b/scripts/configs/predicatorv3/envs/all.yaml @@ -418,11 +418,12 @@ ENVS: # in the codebase; path fidelity is worth the ~2x steps per # motion-planned phase (episodes stay far under the horizon). pybullet_birrt_path_subsample_ratio: 1 - # Right after a pick, the planning sim's reconstruction of the - # held object (grasp-transform capture + orientation round-trip) - # occasionally models it up to ~9 mm into the table it was just - # lifted off -- physically impossible for a 2-3 cm lift, but it - # rejects the next Place's start config. Deepen the shallow-held - # allowance past the artifact (start contacts only; the first - # motion is upward). - pybullet_birrt_shallow_held_contact_margin: -0.012 + # Right after a grasp/pick, the planning-time model of the held + # object occasionally shows it up to ~14 mm into the table it is + # resting on or was just lifted off (a flaky reconstruction / + # transient artifact -- physically impossible for a lifted + # block), which rejects the next phase's start config. Deepen + # the shallow-held allowance past the artifact (start contacts + # only; the first motion is away from the surface, and planning + # away from a genuine wedge is the right recovery anyway). + pybullet_birrt_shallow_held_contact_margin: -0.02 diff --git a/tests/approaches/test_oracle_process_planning_bridge.py b/tests/approaches/test_oracle_process_planning_bridge.py index 141605789..98ce98e68 100644 --- a/tests/approaches/test_oracle_process_planning_bridge.py +++ b/tests/approaches/test_oracle_process_planning_bridge.py @@ -49,10 +49,10 @@ def _oracle_bridge_config() -> dict: # default 1 mm margin turns those into unrecoverable BiRRT # start/goal rejections. "pybullet_birrt_contact_margin": -0.005, - # Post-pick reconstruction artifacts can model the held object - # up to ~9 mm into its pick surface at the next phase's start + # Post-grasp modeling artifacts can show the held object up + # to ~14 mm into its pick surface at the next phase's start # config; allow escaping those (start contacts only). - "pybullet_birrt_shallow_held_contact_margin": -0.012, + "pybullet_birrt_shallow_held_contact_margin": -0.02, # Each Wait ends on the FIRST atom change, so a plan waiting on # several concurrent cures can need a cheap replan for the tail # (which reduces to "Wait until the remaining joint cures"). From b2bfd8eec4ffa63c8a8daece699c31046ef3dc1d Mon Sep 17 00:00:00 2001 From: Yichao Liang Date: Mon, 17 Aug 2026 13:18:11 -0400 Subject: [PATCH 09/30] bridge: kill weld creep and the systematic place landing bias Two structural fixes for why agent-planned bridges came out bent and open-loop plans kept invalidating themselves: 1. Weld anti-creep. A PyBullet JOINT_FIXED constraint between two table-resting bodies skates 7-9 mm and up to 0.13 rad of yaw per 200 idle steps (invariant to maxForce, erp, pair-collision filtering, and a zero-error anchor; present even for a pair welded 5 cm apart), while unwelded pairs move < 1.5 mm. _relax_resting_welds re-anchors every weld at the current snapped pose each step while its assembly is quiescent, unheld, and untouched by the robot, so the solver never accumulates an error to fight; under load the anchor holds and the weld stays rigid. Welds are also created with a zero-error child teleport and welded partners no longer collide with each other (restored on weld removal). Drift is now 0.0-0.1 mm / 200 steps. 2. Verified place release. Plant sag (position control under gravity + payload) walks the settle-to-contact stroke ~15 mm toward the robot base, and the stroke releases at FIRST contact, so every robot placement landed with a repeatable ~15 mm bias the cure gates tolerate but that bends every butt row. The settle phase now verifies the held object is within 4 mm of the commanded xy before opening; on failure it rewinds to the descend phase and re-runs the approach with a learned aim offset that targets upstream of the measured drift (feedforward, so the servo stays unstrained -- a strained anti-bias servo drags the released block through the finger pads when the grasp constraint drops). Landings are now within ~2-4 mm and a butt placement leaves its neighbor untouched. Supporting fixes: the rewound descend allows shallow held-object start contacts (a rewind legitimately starts resting on the support); a failed rewind degrades to best-effort release instead of aborting; and run_motion_planning's contact-partner scan now re-evaluates bodies at both endpoints (a butt-joint neighbor 3.1 mm away at the start but 1.8 mm at the re-aimed goal was permanently classified a bystander and its own goal proximity rejected the plan). Verified: weld drift probes at 0.0-0.1 mm; place accuracy probes at 2-4 mm across the workspace; the full glue-cure-weld chain drift-free across five Waits; demonstrator sweep 22/24 with all experiment- critical instances 8/8; full suite 1486 passed. --- predicators/envs/pybullet_bridge.py | 133 ++++++++++- .../ground_truth_models/bridge/options.py | 9 + .../skill_factories/base.py | 210 ++++++++++++++++-- .../skill_factories/move_to.py | 14 ++ .../skill_factories/place.py | 70 +++++- .../pybullet_helpers/motion_planning.py | 10 +- tests/envs/test_pybullet_bridge.py | 62 +++++- 7 files changed, 471 insertions(+), 37 deletions(-) diff --git a/predicators/envs/pybullet_bridge.py b/predicators/envs/pybullet_bridge.py index acd54f679..56f84bbce 100644 --- a/predicators/envs/pybullet_bridge.py +++ b/predicators/envs/pybullet_bridge.py @@ -394,6 +394,10 @@ def __init__(self, use_gui: bool = False, **kwargs: Any) -> None: # PyBullet constraint id. Must exist before super().__init__ # (reset paths may call _set_domain_specific_state). self._weld_constraints: Dict[FrozenSet[int], int] = {} + # Per-weld creation arguments (parent, child, ideal_dz), kept so + # a resting weld can be re-anchored (see _relax_resting_welds). + self._weld_meta: Dict[FrozenSet[int], Tuple[int, int, + Optional[float]]] = {} # Glue-patch visual bodies: block name -> face -> body id. self._glue_patch_ids: Dict[str, Dict[str, int]] = {} @@ -959,6 +963,36 @@ def _create_weld(self, (0.0, 0.0, 0.0, 1.0)) _, rel_orn = p.multiplyTransforms((0.0, 0.0, 0.0), inv_orn, (0.0, 0.0, 0.0), orn_b_ideal) + # Teleport the child onto the EXACT pose the constraint will + # enforce (parent's ACTUAL frame composed with the snapped + # relative transform) and zero both bodies' velocities, so the + # constraint starts with zero error. Without this, the solver + # spends every subsequent step pulling the pair toward the + # snapped frame while table contact resists, and the rectified + # micro-vibration SKATES the welded assembly across the table + # (measured 2-10 mm and up to 0.08 rad yaw per 200 idle steps; + # unwelded pairs move < 1 mm). The teleport is mm/mrad scale -- + # exactly the snap distance. + child_pos, child_orn = p.multiplyTransforms(pos_a, orn_a, rel_pos, + rel_orn) + p.resetBasePositionAndOrientation( + body_b, + child_pos, + child_orn, + physicsClientId=self._physics_client_id) + for body in (body_a, body_b): + p.resetBaseVelocity(body, (0.0, 0.0, 0.0), (0.0, 0.0, 0.0), + physicsClientId=self._physics_client_id) + # Welded partners must not collide with each other: the box + # collision margin keeps the flush faces in permanent contact, + # and the contact solver fighting the weld is the other motor + # of the same skating creep. Re-enabled in _remove_weld. + p.setCollisionFilterPair(body_a, + body_b, + -1, + -1, + 0, + physicsClientId=self._physics_client_id) cid = p.createConstraint(parentBodyUniqueId=body_a, parentLinkIndex=-1, childBodyUniqueId=body_b, @@ -975,6 +1009,7 @@ def _create_weld(self, maxForce=self.weld_max_force, physicsClientId=self._physics_client_id) self._weld_constraints[key] = cid + self._weld_meta[key] = (body_a, body_b, ideal_dz) def _desired_weld_pairs( self, @@ -1021,13 +1056,100 @@ def _sync_welds_to_state(self, state: State) -> None: desired = self._desired_weld_pairs(state) for key in list(self._weld_constraints): if key not in desired: - p.removeConstraint(self._weld_constraints[key], - physicsClientId=self._physics_client_id) - del self._weld_constraints[key] + self._remove_weld(key) for key, (body_a, body_b, ideal_dz) in desired.items(): if key not in self._weld_constraints: self._create_weld(body_a, body_b, ideal_dz=ideal_dz) + # Quiescence gates for weld re-anchoring (see _relax_resting_welds): + # creep velocities are ~0.5 mm/s and ~4 mrad/s; real dynamics (drops, + # pushes, carried swings) are orders of magnitude above these. + weld_relax_max_lin_vel: ClassVar[float] = 0.02 # m/s + weld_relax_max_ang_vel: ClassVar[float] = 0.2 # rad/s + + def _relax_resting_welds(self) -> None: + """Re-anchor every weld whose assembly is resting free. + + A PyBullet JOINT_FIXED constraint between two table-resting + bodies is never quiescent: each body settles into its own + contact, the constraint accumulates sub-mm error, and the + correction impulses rectify (through friction) into a steady + skate -- measured 7-9 mm and up to 0.13 rad of yaw per 200 idle + steps, invariant to maxForce, erp, pair-collision filtering and + a zero-error anchor at creation, and present even for a welded + pair 5 cm apart. Unwelded pairs in the same layout move < 1 mm. + + The fix breaks the error-accumulation loop: while every member + of a welded assembly is quiescent, not held, and not touched by + the robot, each weld is rebuilt at the current snapped relative + pose every step, so the solver never has an error to fight and + the assembly behaves like resting free bodies (which are + stable). Under load -- carried, pushed, mid-drop -- the gates + fail and the anchor holds, keeping the weld fully rigid exactly + when rigidity matters. The relative-geometry ratchet this + introduces is the free drift of resting bodies (sub-mm over + hundreds of steps), not the skate. + """ + if not self._weld_constraints: + return + # Connected components over the weld graph. + adjacency: Dict[int, Set[int]] = {} + for key in self._weld_constraints: + body_a, body_b = tuple(key) + adjacency.setdefault(body_a, set()).add(body_b) + adjacency.setdefault(body_b, set()).add(body_a) + seen: Set[int] = set() + for root in list(adjacency): + if root in seen: + continue + component = {root} + frontier = [root] + while frontier: + for nxt in adjacency[frontier.pop()]: + if nxt not in component: + component.add(nxt) + frontier.append(nxt) + seen |= component + if self._held_obj_id is not None and \ + self._held_obj_id in component: + continue + resting = True + for body in component: + lin, ang = p.getBaseVelocity( + body, physicsClientId=self._physics_client_id) + if np.linalg.norm(lin) > self.weld_relax_max_lin_vel or \ + np.linalg.norm(ang) > self.weld_relax_max_ang_vel: + resting = False + break + if p.getContactPoints(self._pybullet_robot.robot_id, + body, + physicsClientId=self._physics_client_id): + resting = False + break + if not resting: + continue + for key in list(self._weld_constraints): + if not key <= component: + continue + body_a, body_b, ideal_dz = self._weld_meta[key] + self._remove_weld(key) + self._create_weld(body_a, body_b, ideal_dz=ideal_dz) + + def _remove_weld(self, key: FrozenSet[int]) -> None: + """Tear down one weld: remove the constraint and restore the pair's + collision (disabled at creation; the blocks are separate objects again + after a planner backtrack to a pre-weld state).""" + cid = self._weld_constraints.pop(key) + self._weld_meta.pop(key, None) + p.removeConstraint(cid, physicsClientId=self._physics_client_id) + body_a, body_b = tuple(key) + p.setCollisionFilterPair(body_a, + body_b, + -1, + -1, + 1, + physicsClientId=self._physics_client_id) + def get_welded_partner_transforms( self, body_id: int ) -> Dict[int, Tuple[Tuple[float, ...], Tuple[float, ...]]]: @@ -1149,7 +1271,10 @@ def _domain_specific_step(self) -> None: if cure >= self.cure_threshold: self._latch_joint(state, blk, face, mate) - # 3. Visuals. + # 3. Anti-creep: re-anchor welds whose assembly rests free. + self._relax_resting_welds() + + # 4. Visuals. self._update_glue_patches(state) def _find_mate(self, state: State, blk: Object, diff --git a/predicators/ground_truth_models/bridge/options.py b/predicators/ground_truth_models/bridge/options.py index e83d8d904..f85234ecf 100644 --- a/predicators/ground_truth_models/bridge/options.py +++ b/predicators/ground_truth_models/bridge/options.py @@ -194,6 +194,15 @@ def _get_bottle_grasp_pose( # clearance (the seat's 20 mm) with margin; table places # settle only their 2-3 mm. settle_to_contact_depth=0.03, + # Verified release: the settle stroke ends at FIRST contact, + # and plant sag (position control under gravity + payload) + # was measured walking that contact point ~15 mm toward the + # robot base -- a systematic landing bias the cure gates + # tolerate but that bends every butt row. Before opening, + # require the held block within 4 mm of the commanded xy; + # otherwise lift back to release_z and re-descend, aiming + # upstream of the measured (repeatable) drift. + verify_xy_tol=0.004, ) # -- MoveTo (generic move-through-pose) ------------------------------ diff --git a/predicators/ground_truth_models/skill_factories/base.py b/predicators/ground_truth_models/skill_factories/base.py index 9277f2237..38750117c 100644 --- a/predicators/ground_truth_models/skill_factories/base.py +++ b/predicators/ground_truth_models/skill_factories/base.py @@ -278,6 +278,8 @@ def _fmt_option_params(params: Array) -> str: _STROKE_BEST_KEY = "stroke_best_{}" # gentle stroke: best EE distance _STROKE_NOPROG_KEY = "stroke_noprog_{}" # gentle stroke: no-progress steps _IK_STALL_COUNT_KEY = "ik_stall_count_{}" # steps since last improvement +_AIM_OFFSET_KEY = "aim_offset" # option-scoped learned xy aim (meters) +_PHASE_RETRY_KEY = "phase_retries_{}" # verified-advance retries used @dataclass @@ -368,6 +370,23 @@ class Phase: # - a final-phase stroke keeps the incremental-IK stall abort # (see _check_ik_stall) as its escape instead. max_step_norm: Optional[float] = None + # Verified advancement: when set, this phase only advances (on its + # terminal condition OR a gentle stroke's give-up) if verify_fn + # returns True on the current state. When it returns False and + # retry budget remains, the skill REWINDS to the phase named + # retry_to_phase (clearing the rewound phases' cached trajectories + # and counters, but keeping any learned stroke bias) and re-runs + # from there. Use for outcome-critical phases whose success the + # terminal condition alone cannot certify -- e.g. a place's + # settle-to-contact terminates on FIRST contact, which under a + # plant-sag deflection can happen centimeters from the commanded + # spot; verifying the held object's xy before release converts + # that into a lift-and-re-descend. After max_retries unverified + # attempts the phase advances anyway (best-effort). + verify_fn: Optional[Callable[[State, Sequence[Object], Array, SkillConfig], + bool]] = None + retry_to_phase: Optional[str] = None + max_retries: int = 0 class PhaseSkill: @@ -452,6 +471,10 @@ def _policy(self, state: State, memory: Dict, objects: Sequence[Object], memory[dwell_key] = dwelled + 1 return self._execute_phase(phase, state, memory, objects, params) + retry_action = self._maybe_retry_phase(phase, state, memory, + objects, params) + if retry_action is not None: + return retry_action phase_idx += 1 memory["phase_idx"] = phase_idx if phase_idx >= len(self._phases): @@ -522,7 +545,8 @@ def _phase_is_terminal(self, phase: Phase, state: State, memory: Dict, if phase.use_motion_planning: return self._birrt_phase_is_terminal(phase, state, memory, objects, params) - return self._ik_phase_is_terminal(phase, state, objects, params) + return self._ik_phase_is_terminal(phase, state, memory, objects, + params) def _birrt_phase_is_terminal(self, phase: Phase, state: State, memory: Dict, objects: Sequence[Object], @@ -545,7 +569,8 @@ def _birrt_phase_is_terminal(self, phase: Phase, state: State, traj = memory[traj_key] if traj is None: # BiRRT failed; use distance-based terminal (IK fallback mode). - return self._ik_phase_is_terminal(phase, state, objects, params) + return self._ik_phase_is_terminal(phase, state, memory, objects, + params) # All waypoints consumed — fall back to position-based terminal so # the phase doesn't end until the robot has actually converged to the @@ -553,20 +578,45 @@ def _birrt_phase_is_terminal(self, phase: Phase, state: State, # and IK inaccuracy means the final waypoint may not exactly match # the target Cartesian pose). if memory[step_key] >= len(traj): - return self._ik_phase_is_terminal(phase, state, objects, params) + return self._ik_phase_is_terminal(phase, state, memory, objects, + params) return False - def _ik_phase_is_terminal(self, phase: Phase, state: State, + def _ik_phase_is_terminal(self, phase: Phase, state: State, memory: Dict, objects: Sequence[Object], params: Array) -> bool: """Distance-based terminal for incremental IK phases.""" - current_pose, target_pose, _ = phase.target_fn(state, objects, params, - self._config) + current_pose, target_pose, _ = self._phase_targets( + phase, state, memory, objects, params) squared_dist = np.sum( np.square(np.subtract(current_pose.position, target_pose.position))) return bool(squared_dist < self._config.move_to_pose_tol) + def _phase_targets(self, phase: Phase, state: State, memory: Dict, + objects: Sequence[Object], + params: Array) -> Tuple[Pose, Pose, str]: + """``phase.target_fn`` with the option's learned aim offset applied to + the target xy. + + The offset (see the aim learning in ``_maybe_retry_phase``) re- + aims EVERY move target of the option upstream of a measured + repeatable plant drift, so the approach phases arrive pre- + compensated and the final contact stroke descends nearly + vertically instead of having to outrun the sag laterally (a 3 + mm-clamped stroke cannot: its lateral command component tops out + below the ~2 mm/step sag). + """ + current_pose, target_pose, finger_status = phase.target_fn( + state, objects, params, self._config) + aim = memory.get(_AIM_OFFSET_KEY) + if aim is not None: + target_pose = Pose( + (target_pose.position[0] + aim[0], + target_pose.position[1] + aim[1], target_pose.position[2]), + target_pose.orientation) + return current_pose, target_pose, finger_status + def _check_ik_stall(self, phase: Phase, state: State, memory: Dict, objects: Sequence[Object], params: Array) -> None: """Abort the option when incremental IK stops making progress. @@ -578,8 +628,8 @@ def _check_ik_stall(self, phase: Phase, state: State, memory: Dict, can otherwise never fire, leaving the arm thrashing until the episode horizon). """ - current_pose, target_pose, _ = phase.target_fn(state, objects, params, - self._config) + current_pose, target_pose, _ = self._phase_targets( + phase, state, memory, objects, params) dist = float( np.linalg.norm( np.subtract(current_pose.position, target_pose.position))) @@ -604,6 +654,107 @@ def _check_ik_stall(self, phase: Phase, state: State, memory: Dict, f"{_fmt_option_params(params)}); aborting option." f"{contact_report}") + def _maybe_retry_phase(self, + phase: Phase, + state: State, + memory: Dict, + objects: Sequence[Object], + params: Array, + learn_aim: bool = True) -> Optional[Action]: + """Verified advancement (see ``Phase.verify_fn``): when the finishing + phase fails its verification and retry budget remains, rewind to + ``phase.retry_to_phase`` and return that phase's next action; otherwise + return None (advance normally). + + With ``learn_aim`` (a gentle stroke's terminal-time rewind), the + stroke's xy error to its own target is folded into a per-phase + AIM OFFSET before rewinding: the retried stroke steps toward + ``target + aim``, so a repeatable plant drift (gravity+payload + sag walks a bridge settle stroke ~15 mm toward the robot base, + repeatable to ~1 mm) is cancelled FEEDFORWARD. Aiming upstream + of the sag keeps the servo unstrained -- an earlier attempt at + per-step anti-bias servoing held the stroke on target but stored + ~8 mm of command strain, and the arm's relaxation snap at the + instant the grasp constraint dropped dragged the released object + ~5 mm through the still-close finger pads. Give-up rewinds + (blocked strokes) pass ``learn_aim=False``: their error vector + measures the obstruction, not the sag. + """ + if phase.verify_fn is None: + return None + if phase.verify_fn(state, objects, params, self._config): + return None + retry_key = _PHASE_RETRY_KEY.format(id(phase)) + used = memory.get(retry_key, 0) + if used >= phase.max_retries: + logging.debug( + "[%s/%s] verification failed after %d retries; " + "advancing best-effort.", self._name, phase.name, used) + return None + memory[retry_key] = used + 1 + if learn_aim and phase.max_step_norm is not None: + # Error measured against the TRUE (unaimed) target: the aim + # update law is aim -= (current - true_target), which + # accumulates correctly across retries. + current_pose, target_pose, _ = phase.target_fn( + state, objects, params, self._config) + aim = np.array(memory.get(_AIM_OFFSET_KEY, (0.0, 0.0)), + dtype=np.float64) + err_xy = np.subtract(current_pose.position[:2], + target_pose.position[:2]) + aim = aim - err_xy + aim_norm = float(np.linalg.norm(aim)) + if aim_norm > self._stroke_aim_max: + aim = aim * (self._stroke_aim_max / aim_norm) + memory[_AIM_OFFSET_KEY] = (float(aim[0]), float(aim[1])) + logging.debug( + "[%s/%s] landing error (%.1f, %.1f) mm; retry aim " + "offset (%.1f, %.1f) mm.", self._name, phase.name, + err_xy[0] * 1000, err_xy[1] * 1000, aim[0] * 1000, + aim[1] * 1000) + assert phase.retry_to_phase is not None + target_idx = next(i for i, ph in enumerate(self._phases) + if ph.name == phase.retry_to_phase) + cur_idx = memory["phase_idx"] + assert target_idx <= cur_idx + for ph in self._phases[target_idx:cur_idx + 1]: + self._clear_phase_memory(ph, memory) + memory["phase_idx"] = target_idx + logging.debug( + "[%s/%s] verification failed; rewinding to phase " + "%d: %s (retry %d/%d).", self._name, phase.name, target_idx, + phase.retry_to_phase, used + 1, phase.max_retries) + try: + return self._execute_phase(self._phases[target_idx], state, memory, + objects, params) + except utils.OptionExecutionFailure as e: + # The rewound phase could not even start (e.g. the re-aimed + # descend goal of a deliberately flush placement now models + # in collision). A retry is opportunistic: degrade to the + # unverified advance (release where the stroke ended, the + # pre-verification behavior) instead of aborting the option. + logging.debug( + "[%s/%s] retry rewind failed (%s); advancing " + "best-effort.", self._name, phase.name, e) + memory["phase_idx"] = cur_idx + return None + + def _clear_phase_memory(self, phase: Phase, memory: Dict) -> None: + """Drop a phase's cached trajectory and progress counters so a rewound + phase re-plans and re-tracks from scratch. + + Deliberately KEEPS the option's aim offset (_AIM_OFFSET_KEY, + option-scoped): it is the learned plant drift, and re-aiming + the whole approach by it is exactly what makes a retried place + land on target. + """ + pid = id(phase) + for key_fmt in (_BIRRT_TRAJ_KEY, _BIRRT_STEP_KEY, _BIRRT_FINGER_KEY, + _BIRRT_HOLD_KEY, _FINGER_TARGET_KEY, _DWELL_COUNT_KEY, + _STROKE_BEST_KEY, _STROKE_NOPROG_KEY, + _IK_STALL_BEST_KEY, _IK_STALL_COUNT_KEY): + memory.pop(key_fmt.format(pid), None) + # ------------------------------------------------------------------ # Phase execution # ------------------------------------------------------------------ @@ -626,7 +777,7 @@ def _execute_move(self, phase: Phase, state: State, memory: Dict, if phase.max_step_norm is not None: return self._execute_gentle_stroke(phase, state, memory, objects, params) - return self._execute_move_ik(phase, state, objects, params) + return self._execute_move_ik(phase, state, memory, objects, params) def _execute_gentle_stroke(self, phase: Phase, state: State, memory: Dict, objects: Sequence[Object], @@ -655,8 +806,8 @@ def _execute_gentle_stroke(self, phase: Phase, state: State, memory: Dict, if phase_idx >= len(self._phases) - 1: self._check_ik_stall(phase, state, memory, objects, params) else: - current_pose, target_pose, _ = phase.target_fn( - state, objects, params, self._config) + current_pose, target_pose, _ = self._phase_targets( + phase, state, memory, objects, params) dist = float( np.linalg.norm( np.subtract(current_pose.position, target_pose.position))) @@ -669,6 +820,20 @@ def _execute_gentle_stroke(self, phase: Phase, state: State, memory: Dict, else: memory[count_key] = memory.get(count_key, 0) + 1 if memory[count_key] >= self._gentle_stroke_giveup_steps: + # A blocked stroke is exactly what verified + # advancement exists for: prefer a rewind (lift and + # re-approach) over advancing from wherever it got + # stuck, while retry budget remains. No aim + # learning here: a blocked stroke's error measures + # the obstruction, not the plant's sag. + retry_action = self._maybe_retry_phase(phase, + state, + memory, + objects, + params, + learn_aim=False) + if retry_action is not None: + return retry_action memory["phase_idx"] = phase_idx + 1 nxt = self._phases[phase_idx + 1] logging.debug( @@ -679,7 +844,7 @@ def _execute_gentle_stroke(self, phase: Phase, state: State, memory: Dict, nxt.name) return self._execute_phase(nxt, state, memory, objects, params) - action = self._execute_move_ik(phase, state, objects, params) + action = self._execute_move_ik(phase, state, memory, objects, params) pb_state = cast(utils.PyBulletState, state) robot = self._config.robot finger_idxs = (robot.left_finger_joint_idx, @@ -751,8 +916,8 @@ def _maybe_drive_base(self, phase: Phase, state: State, memory: Dict, # singularity and makes the push wander off target. target_bx, target_by = home_xy else: - _, target_pose, _ = phase.target_fn(state, objects, params, - self._config) + _, target_pose, _ = self._phase_targets(phase, state, memory, + objects, params) home_x = home_xy[0] if home_xy is not None else ( self._config.robot_home_pos[0] if self._config.robot_home_pos is not None else float(cur_x)) @@ -847,8 +1012,8 @@ def _execute_move_birrt(self, phase: Phase, state: State, memory: Dict, if traj_key not in memory: # --- First call: plan the trajectory. --- - _, target_pose, finger_status = phase.target_fn( - state, objects, params, self._config) + _, target_pose, finger_status = self._phase_targets( + phase, state, memory, objects, params) memory[finger_key] = finger_status self._last_plan_diagnostics = [] @@ -916,7 +1081,7 @@ def _execute_move_birrt(self, phase: Phase, state: State, memory: Dict, if traj is None: # BiRRT failed — fall back to incremental IK. self._check_ik_stall(phase, state, memory, objects, params) - return self._execute_move_ik(phase, state, objects, params) + return self._execute_move_ik(phase, state, memory, objects, params) # --- Pop next waypoint from cached trajectory. --- step = memory[step_key] @@ -926,7 +1091,7 @@ def _execute_move_birrt(self, phase: Phase, state: State, memory: Dict, # to the exact target pose (BiRRT's IK solution may be slightly # off from the target Cartesian pose). self._check_ik_stall(phase, state, memory, objects, params) - return self._execute_move_ik(phase, state, objects, params) + return self._execute_move_ik(phase, state, memory, objects, params) finger_idx_l = robot.left_finger_joint_idx finger_idx_r = robot.right_finger_joint_idx @@ -1537,15 +1702,18 @@ def _check(joints: JointPositions, label: str) -> None: # gives up and advances to the next phase (see # _execute_gentle_stroke). _gentle_stroke_giveup_steps: ClassVar[int] = 8 + # Safety clamp on the learned aim offset (meters); see + # _maybe_retry_phase's aim learning and _phase_targets. + _stroke_aim_max: ClassVar[float] = 0.03 - def _execute_move_ik(self, phase: Phase, state: State, + def _execute_move_ik(self, phase: Phase, state: State, memory: Dict, objects: Sequence[Object], params: Array) -> Action: """Execute a MOVE_TO_POSE phase using incremental IK delta-stepping.""" pb_state = cast(utils.PyBulletState, state) robot = self._config.robot robot.set_joints(pb_state.joint_positions) - current_pose, target_pose, finger_status = phase.target_fn( - state, objects, params, self._config) + current_pose, target_pose, finger_status = self._phase_targets( + phase, state, memory, objects, params) try: action = self._move_ik_action(phase, pb_state, current_pose, target_pose, finger_status) diff --git a/predicators/ground_truth_models/skill_factories/move_to.py b/predicators/ground_truth_models/skill_factories/move_to.py index 10a7260fe..5b0f04c3e 100644 --- a/predicators/ground_truth_models/skill_factories/move_to.py +++ b/predicators/ground_truth_models/skill_factories/move_to.py @@ -155,6 +155,10 @@ def make_move_to_phase( [State, Sequence[Object], Array, SkillConfig], bool]] = None, max_step_norm: Optional[float] = None, dwell_steps: int = 0, + verify_fn: Optional[Callable[[State, Sequence[Object], Array, SkillConfig], + bool]] = None, + retry_to_phase: Optional[str] = None, + max_retries: int = 0, ) -> Phase: """Create a MOVE_TO_POSE phase for use in a ``PhaseSkill``. @@ -187,6 +191,13 @@ def make_move_to_phase( policy steps before advancing (see ``Phase.dwell_steps``), for "move there and DWELL" semantics such as sustained- proximity glue application. + verify_fn: Optional verified-advancement predicate forwarded to + the ``Phase`` (see ``Phase.verify_fn``): the phase only + advances when it returns True; otherwise the skill rewinds + to ``retry_to_phase`` up to ``max_retries`` times. + retry_to_phase: Name of the phase to rewind to on a failed + verification. + max_retries: Verification retry budget (see ``Phase``). Returns: A ``Phase`` that can be included in a ``PhaseSkill``. @@ -251,4 +262,7 @@ def _target_fn( use_motion_planning=plan_motion, max_step_norm=max_step_norm, dwell_steps=dwell_steps, + verify_fn=verify_fn, + retry_to_phase=retry_to_phase, + max_retries=max_retries, ) diff --git a/predicators/ground_truth_models/skill_factories/place.py b/predicators/ground_truth_models/skill_factories/place.py index fc3976d59..3a799b484 100644 --- a/predicators/ground_truth_models/skill_factories/place.py +++ b/predicators/ground_truth_models/skill_factories/place.py @@ -117,6 +117,8 @@ def create_place_skill( compensate_held_offset: bool = False, compensate_held_z: bool = False, settle_to_contact_depth: Optional[float] = None, + verify_xy_tol: Optional[float] = None, + verify_max_retries: int = 2, ) -> ParameterizedOption: """Create a multi-phase place skill that releases a held object. @@ -190,6 +192,19 @@ def create_place_skill( drop. Note the release-clearance check still validates the finger-opening sweep at ``release_z``, an upper bound of the actual release pose. + verify_xy_tol: If set (requires ``settle_to_contact_depth``), + verify BEFORE releasing that the still-held object's xy is + within this many meters of the commanded ``(target_x, + target_y)``. On failure the skill rewinds to the descend + phase (lifting the held object back to ``release_z``) and + re-descends, up to ``verify_max_retries`` times. The settle + stroke releases at FIRST contact, and plant sag (position + control under gravity + payload) can walk that contact + point ~15 mm from the commanded spot; the rewind learns an + aim offset from the measured error, so the retried stroke + aims upstream of the (repeatable) sag and lands on target + with an unstrained servo. + verify_max_retries: Retry budget for the verification. Returns: A ``ParameterizedOption`` implementing the place skill. @@ -363,22 +378,61 @@ def _settled_or_at_depth( "Descend" if use_move_above else "MoveToDrop", _drop_pose, "closed", - allow_shallow_held_object_contacts=not use_move_above, + # Without a move-above, this is the post-pick first move + # (see above). With a settle stroke, a failed verification + # rewinds HERE while the held object rests on its support + # (the stroke ended at contact); that start contact is + # escapable -- the first motion is back up to release_z. + allow_shallow_held_object_contacts=(not use_move_above + or settle_to_contact_depth + is not None), check_release_clearance=True)) if settle_to_contact_depth is not None: + + def _held_xy_on_target( + state: State, + objects: Sequence[Object], + params: Array, + cfg: SkillConfig, + ) -> bool: + # The held object itself (not the EE) must sit on the + # commanded (x, y) before we let go. Nothing held (already + # released, or the grasp broke) is unverifiable: pass. + del cfg # unused + assert verify_xy_tol is not None + tx, ty = float(params[0]), float(params[1]) + robot_obj = objects[0] + for obj in state: + if obj == robot_obj or \ + "is_held" not in obj.type.feature_names: + continue + if state.get(obj, "is_held") > 0.5: + err = float( + np.hypot( + state.get(obj, "x") - tx, + state.get(obj, "y") - ty)) + return err <= verify_xy_tol + return True + # Gentle stroke: 3 mm steps bound the post-contact overshoot # (contact is only observed at the next policy step) and arm # the joint-jump guard -- single-shot IK once answered a plain # 2 cm descent with a wrist-flipped branch, and the flipped # retreat then batted the released block across the table. phases.append( - make_move_to_phase("SettleToContact", - _settle_pose, - "closed", - expect_contact=True, - use_motion_planning=False, - terminal_fn=_settled_or_at_depth, - max_step_norm=0.003)) + make_move_to_phase( + "SettleToContact", + _settle_pose, + "closed", + expect_contact=True, + use_motion_planning=False, + terminal_fn=_settled_or_at_depth, + max_step_norm=0.003, + verify_fn=(_held_xy_on_target + if verify_xy_tol is not None else None), + retry_to_phase=("Descend" if use_move_above else "MoveToDrop"), + max_retries=(verify_max_retries + if verify_xy_tol is not None else 0))) if partial_release: phases.extend([ Phase( diff --git a/predicators/pybullet_helpers/motion_planning.py b/predicators/pybullet_helpers/motion_planning.py index da7fc55ee..e1b733aea 100644 --- a/predicators/pybullet_helpers/motion_planning.py +++ b/predicators/pybullet_helpers/motion_planning.py @@ -185,7 +185,15 @@ def _set_state(pt: JointPositions) -> None: bystander_clearance, physicsClientId=physics_client_id): contact_partners.add(body) - if not held_assembly or body in held_near_endpoint: + # Evaluate held proximity at BOTH endpoints, even for a + # body already seen near the other one: partner status + # (within the clearance) at EITHER endpoint must win. + # Skipping bodies already in held_near_endpoint once + # made a butt-joint neighbor 3.1 mm away at the start + # but 1.8 mm away at the (re-aimed) goal a permanent + # bystander, and its own goal proximity then rejected + # the plan. + if not held_assembly or body in contact_partners: continue held_dists: List[float] = [] for assembly_body, _ in held_assembly: diff --git a/tests/envs/test_pybullet_bridge.py b/tests/envs/test_pybullet_bridge.py index ec6a66c7e..c6136630b 100644 --- a/tests/envs/test_pybullet_bridge.py +++ b/tests/envs/test_pybullet_bridge.py @@ -220,10 +220,14 @@ def run_option(opt, objs, params): run_option(options["Place"], [env._robot], [tx, ty, resting_z + 0.008, 0.0]) # Landed at resting height (no residual drop), near the target, - # without spinning. + # without spinning. The 6 mm xy bound covers the verified + # release: plant sag walks an unverified settle stroke ~15 mm + # toward the robot base, and the verify-and-re-aim retry (see + # create_place_skill's verify_xy_tol) is what keeps landings + # within tolerance. assert abs(state.get(span1, "z") - resting_z) < 0.002 - assert abs(state.get(span1, "x") - tx) < 0.01 - assert abs(state.get(span1, "y") - ty) < 0.01 + assert abs(state.get(span1, "x") - tx) < 0.006 + assert abs(state.get(span1, "y") - ty) < 0.006 assert abs(state.get(span1, "yaw")) < 0.03 finally: import pybullet as p # pylint: disable=import-outside-toplevel @@ -320,3 +324,55 @@ def test_seat_weld_holds_pose(env_and_task): assert abs(final.get(obj, feat) - latched.get(obj, feat)) < 0.01 assert abs(final.get(leg, "pitch") + np.pi / 2) < 0.05 assert abs(final.get(span, "pitch")) < 0.05 + + +def test_welded_pair_does_not_creep(env_and_task): + """A freshly welded resting pair must stay put while the scene idles. + + Regression: a PyBullet JOINT_FIXED constraint between two + table-resting bodies accumulates sub-mm error as each body settles + into its own contact, and the correction impulses rectify into a + steady skate -- 7-9 mm and up to 0.13 rad of yaw per 200 idle steps + (unwelded pairs move < 1.5 mm), enough to invalidate every + downstream open-loop placement parameter and bend every row. The + quiescent re-anchoring in _relax_resting_welds must hold the pair + still. + """ + env, task = env_and_task + env._set_state(task.init) + state = env._get_state() + blocks = state.get_objects(env._block_type) + span0 = next(b for b in blocks if b.name == "span0") + span1 = next(b for b in blocks if b.name == "span1") + + s = state.copy() + table_z = s.get(span0, "z") + for blk, x in ((span0, 0.45), (span1, 0.45 + 0.1 + 0.0001)): + s.set(blk, "x", x) + s.set(blk, "y", 1.14) + s.set(blk, "z", table_z) + for feat in ("roll", "pitch", "yaw"): + s.set(blk, feat, 0.0) + s.set(span0, "glue_end_b", 1.0) + for i, blk in enumerate(blocks): + if blk not in (span0, span1): + s.set(blk, "x", 2.0 + 0.2 * i) + s.set(blk, "y", 2.0) + env._set_state(s) + + for _ in range(env.cure_threshold + 5): + env.step(_hold_action(env)) + latched = env._get_state() + assert latched.get(span0, "attached_end_b") == \ + float(env._block_index[span1.name]) + + for _ in range(200): + env.step(_hold_action(env)) + final = env._get_state() + for obj in (span0, span1): + drift = np.hypot( + final.get(obj, "x") - latched.get(obj, "x"), + final.get(obj, "y") - latched.get(obj, "y")) + assert drift < 0.002, f"{obj.name} skated {drift * 1000:.1f} mm" + dyaw = abs(final.get(obj, "yaw") - latched.get(obj, "yaw")) + assert dyaw < 0.01, f"{obj.name} rotated {dyaw:.4f} rad" From 0456f8bfec7d936089a0d3f1ca41e4596de43d24 Mon Sep 17 00:00:00 2001 From: Yichao Liang Date: Mon, 17 Aug 2026 16:39:35 -0400 Subject: [PATCH 10/30] skills: collision-aware goal-config selection across IK branches Goal IK returned the FIRST pose-accurate branch, with no collision awareness. IK branches reach the same end-effector pose with different arm configurations, and they are not collision-equivalent: one grasp branch can sweep a link ~6 cm through a neighboring block while another clears it. Which branch a single solve lands on depends on the IK seed, so goal-config acceptance was a per-seed coin flip -- observed as validation flakiness in the bridge agent runs (a capture candidate reached the goal on 2/3 rollouts and was rejected FLAKY because one decorrelated repeat drew a grasp branch modeled 59 mm inside a standing leg). _solve_goal_ik is now _solve_goal_ik_candidates: it collects ALL distinct pose-accurate limit-clamped branches from the existing multi-seed restarts (deduplicated, current-joints branch first), and run_motion_planning takes them as goal_candidates, planning to the first branch whose goal configuration (and fingers-open variant, when a release follows) passes its collision check. When none passes, the primary branch is kept so failure diagnostics still report the blocking contacts. Verified: skill-factory unit tests updated to the candidates API plus a new branch-collection test; oracle bridge E2E; demonstrator sweep 22/24 with experiment-critical instances 8/8; full suite 1487 passed. --- .../skill_factories/base.py | 58 +++++++++++++------ .../pybullet_helpers/motion_planning.py | 35 +++++++++++ tests/test_skill_factories.py | 46 ++++++++++++--- 3 files changed, 112 insertions(+), 27 deletions(-) diff --git a/predicators/ground_truth_models/skill_factories/base.py b/predicators/ground_truth_models/skill_factories/base.py index 38750117c..f693ed959 100644 --- a/predicators/ground_truth_models/skill_factories/base.py +++ b/predicators/ground_truth_models/skill_factories/base.py @@ -1432,7 +1432,7 @@ def _plan_with_simulator( validate_goal_ik = self._config.ik_validate or (phase is not None and phase.validate_ik) try: - target_joints: JointPositions = self._solve_goal_ik( + goal_candidates = self._solve_goal_ik_candidates( planning_robot, target_pose, pb_state.joint_positions, validate_goal_ik) except InverseKinematicsError: @@ -1442,6 +1442,7 @@ def _plan_with_simulator( "(%.3f, %.3f, %.3f); falling back to incremental IK.", self._name, phase_name, pos[0], pos[1], pos[2]) return None + target_joints: JointPositions = goal_candidates[0] goal_finger_joint = None if phase is not None and phase.check_release_clearance: # Check the width the fingers actually reach at the drop pose: @@ -1471,6 +1472,7 @@ def _plan_with_simulator( unbounded_shallow_bodies=self._sim_table_ids(sim), goal_finger_joint=goal_finger_joint, held_bystander_clearance=self._config.held_bystander_clearance, + goal_candidates=goal_candidates, ) if traj is None and not validate_goal_ik: @@ -1481,21 +1483,21 @@ def _plan_with_simulator( # in-limit branch whose goal configuration is collision-free. sim._set_state(remapped_state) # pylint: disable=protected-access planning_robot.set_joints(pb_state.joint_positions) - validated_target_joints: Optional[JointPositions] = None + validated_candidates: Optional[List[JointPositions]] = None try: - validated_target_joints = self._solve_goal_ik( + validated_candidates = self._solve_goal_ik_candidates( planning_robot, target_pose, pb_state.joint_positions, validate=True) except InverseKinematicsError: pass - if validated_target_joints is not None and \ - validated_target_joints != target_joints: + if validated_candidates is not None and \ + validated_candidates != goal_candidates: traj = run_motion_planning( robot=planning_robot, initial_positions=pb_state.joint_positions, - target_positions=validated_target_joints, + target_positions=validated_candidates[0], collision_bodies=collision_bodies, seed=CFG.seed, physics_client_id=sim._physics_client_id, # pylint: disable=protected-access @@ -1509,9 +1511,10 @@ def _plan_with_simulator( goal_finger_joint=goal_finger_joint, held_bystander_clearance=( self._config.held_bystander_clearance), + goal_candidates=validated_candidates, ) if traj is not None: - target_joints = validated_target_joints + target_joints = validated_candidates[0] if traj is None and not expect_contact: self._last_plan_diagnostics = self._log_collision_diagnostics( @@ -1529,10 +1532,11 @@ def _plan_with_simulator( return traj - def _solve_goal_ik(self, planning_robot: SingleArmPyBulletRobot, - target_pose: Pose, current_joints: JointPositions, - validate: bool) -> JointPositions: - """Goal-config IK that is accurate AFTER joint-limit clamping. + def _solve_goal_ik_candidates(self, planning_robot: SingleArmPyBulletRobot, + target_pose: Pose, + current_joints: JointPositions, + validate: bool) -> List[JointPositions]: + """All distinct pose-accurate goal configs, ordered by seed priority. PyBullet IK is a one-shot approximation with no accuracy guarantee (a far seed can miss by centimeters) and it ignores @@ -1548,9 +1552,18 @@ def _solve_goal_ik(self, planning_robot: SingleArmPyBulletRobot, is False, the cheap unvalidated one-shot is tried first and the SAME seed escalates to validated (iterated) IK if it misses. Seeds: the current joints, the home configuration, then - deterministic random in-limit restarts. Raise - ``InverseKinematicsError`` when no attempt produces an - acceptable config. + deterministic random in-limit restarts. + + ALL accepted candidates are returned (deduplicated, seed order + preserved, so the current-joints branch comes first): which arm + BRANCH a single solve lands on is seed-dependent, and branches + are pose-equivalent but not collision-equivalent -- one grasp + branch can sweep a link 6 cm through a neighboring block while + another clears it. ``run_motion_planning`` picks the first + collision-free candidate (see its ``goal_candidates``), turning + that per-seed coin flip into a deterministic choice. Raise + ``InverseKinematicsError`` when no seed produces an acceptable + config. """ limits = list( zip(planning_robot.joint_lower_limits, @@ -1567,6 +1580,7 @@ def _solve_goal_ik(self, planning_robot: SingleArmPyBulletRobot, rng.uniform(cur - np.pi, cur + np.pi)) for (lo, hi), cur in zip(limits, current_joints) ]) + candidates: List[JointPositions] = [] best_err = float("inf") for seed in seeds: for attempt_validate in ((True, ) if validate else (False, True)): @@ -1589,11 +1603,19 @@ def _solve_goal_ik(self, planning_robot: SingleArmPyBulletRobot, np.square( np.subtract(ee_position, target_pose.position)))) if err < self._config.move_to_pose_tol: - return clamped + if not any( + max(abs(a - b) + for a, b in zip(clamped, prior)) < 1e-3 + for prior in candidates): + candidates.append(clamped) + break best_err = min(best_err, err) - raise InverseKinematicsError( - f"Goal IK missed the target pose from all {len(seeds)} seeds " - f"(best squared FK error after limit clamping {best_err:.6f}).") + if not candidates: + raise InverseKinematicsError( + f"Goal IK missed the target pose from all {len(seeds)} seeds " + f"(best squared FK error after limit clamping {best_err:.6f})." + ) + return candidates def _log_collision_diagnostics( self, diff --git a/predicators/pybullet_helpers/motion_planning.py b/predicators/pybullet_helpers/motion_planning.py index e1b733aea..cddddda74 100644 --- a/predicators/pybullet_helpers/motion_planning.py +++ b/predicators/pybullet_helpers/motion_planning.py @@ -30,6 +30,7 @@ def run_motion_planning( unbounded_shallow_bodies: Optional[Collection[int]] = None, goal_finger_joint: Optional[float] = None, held_bystander_clearance: Optional[float] = None, + goal_candidates: Optional[Sequence[JointPositions]] = None, ) -> Optional[Sequence[JointPositions]]: """Run BiRRT to find a collision-free sequence of joint positions. @@ -57,6 +58,13 @@ def run_motion_planning( ``CFG.pybullet_birrt_held_bystander_clearance`` (the wider berth the held object keeps from bodies the path never intends to approach). + ``goal_candidates`` (optional) supplies pose-equivalent IK branches + for the goal; the first collision-free one replaces + ``target_positions`` as the planning goal. Branches reach the same + end-effector pose with different arm configurations, so goal-config + collision is a property of the BRANCH, not the pose -- selecting + among them here removes the per-IK-seed luck from goal acceptance. + ``unbounded_shallow_bodies`` (used with ``allow_shallow_held_object_contacts``): bodies -- static supports like tables -- whose START-state contacts with the held assembly @@ -269,6 +277,33 @@ def _collision_fn(pt: JointPositions) -> bool: return True return False + # Collision-aware goal selection: the caller may supply several + # pose-equivalent IK branches (see _solve_goal_ik_candidates). The + # branches reach the same end-effector pose but differ in arm + # configuration, and they are NOT collision-equivalent: one grasp + # branch can sweep a link ~6 cm through a neighboring block while + # another clears it. Which branch a single IK solve lands on is + # seed-dependent, so goal-config rejection used to be a per-seed + # coin flip (a validation repeat failed with the robot modeled + # 59 mm inside a standing leg at a grasp goal that other repeats + # planned fine). Take the first branch whose goal configuration + # (and fingers-open variant, when a release follows) is collision- + # free; when none passes, keep the primary target so the caller's + # failure diagnostics report its contacts. + if goal_candidates is not None: + for cand in goal_candidates: + cand_list = list(cand) + if _collision_fn(cand_list): + continue + if goal_finger_joint is not None: + cand_release = list(cand_list) + cand_release[robot.left_finger_joint_idx] = goal_finger_joint + cand_release[robot.right_finger_joint_idx] = goal_finger_joint + if _collision_fn(cand_release): + continue + target_positions = cand_list + break + if goal_finger_joint is not None: release_config = list(target_positions) release_config[robot.left_finger_joint_idx] = goal_finger_joint diff --git a/tests/test_skill_factories.py b/tests/test_skill_factories.py index 1c3f6c2f2..0080361f9 100644 --- a/tests/test_skill_factories.py +++ b/tests/test_skill_factories.py @@ -1317,7 +1317,7 @@ def test_progress_resets_counter(self, robot_scene): # --------------------------------------------------------------------------- -# PhaseSkill._solve_goal_ik acceptance logic +# PhaseSkill._solve_goal_ik_candidates acceptance logic # --------------------------------------------------------------------------- @@ -1357,8 +1357,14 @@ def forward_kinematics(self, joints): return Pose((x, y, z - self._one_shot_error_m)) -class TestSolveGoalIk: - """Every accepted goal config must hit the pose under FK.""" +# Seed count inside _solve_goal_ik_candidates: current joints, home, +# then the random restarts. Every seed is tried (branch collection). +_GOAL_IK_NUM_SEEDS = 2 + PhaseSkill._goal_ik_num_restarts # pylint: disable=protected-access + + +class TestSolveGoalIkCandidates: + """Every accepted goal config must hit the pose under FK, and all distinct + pose-accurate branches are collected (deduplicated).""" def _make_skill(self, robot) -> PhaseSkill: config = _make_config(robot) @@ -1385,12 +1391,14 @@ def test_inaccurate_one_shot_escalates_to_validated(self, robot_scene): skill = self._make_skill(robot) target = Pose((0.77, 1.34, 0.55)) fake = _FakeGoalIkRobot(target, one_shot_error_m=0.057) - result = skill._solve_goal_ik( # pylint: disable=protected-access + result = skill._solve_goal_ik_candidates( # pylint: disable=protected-access fake, target, [0.5] * 7, validate=False) - assert result == [0.1] * 7 - assert fake.validated_calls == 1 + # The validated branch is the only accurate one; every seed + # escalates to it and dedup collapses them to one candidate. + assert result == [[0.1] * 7] + assert fake.validated_calls == _GOAL_IK_NUM_SEEDS def test_accurate_one_shot_keeps_fast_path(self, robot_scene): """A one-shot within tolerance is accepted with no validated IK.""" @@ -1398,13 +1406,33 @@ def test_accurate_one_shot_keeps_fast_path(self, robot_scene): skill = self._make_skill(robot) target = Pose((0.77, 1.34, 0.55)) fake = _FakeGoalIkRobot(target, one_shot_error_m=0.002) - result = skill._solve_goal_ik( # pylint: disable=protected-access + result = skill._solve_goal_ik_candidates( # pylint: disable=protected-access fake, target, [0.5] * 7, validate=False) - assert result == [0.2] * 7 + assert result == [[0.2] * 7] assert fake.validated_calls == 0 + def test_distinct_branches_are_all_collected(self, robot_scene): + """Pose-equivalent but distinct arm branches must ALL be returned, in + seed order, so the motion planner can pick the first collision-free one + (branches are not collision-equivalent: one grasp branch swept a link 6 + cm through a neighboring leg while another cleared it).""" + _, robot = robot_scene + skill = self._make_skill(robot) + target = Pose((0.77, 1.34, 0.55)) + fake = _FakeGoalIkRobot(target, one_shot_error_m=0.0) + branches = [[0.1 * (i % 3)] * 7 for i in range(_GOAL_IK_NUM_SEEDS)] + fake.inverse_kinematics = ( # type: ignore + lambda *a, _it=iter(branches), **k: next(_it)) + fake.forward_kinematics = ( # type: ignore + lambda joints: Pose(target.position)) + result = skill._solve_goal_ik_candidates( # pylint: disable=protected-access + fake, + target, [0.5] * 7, + validate=False) + assert result == [[0.0] * 7, [0.1] * 7, [0.2] * 7] + def test_all_branches_inaccurate_raises(self, robot_scene): """When no branch hits the pose, goal IK raises instead of handing BiRRT a wrong goal configuration.""" @@ -1416,7 +1444,7 @@ def test_all_branches_inaccurate_raises(self, robot_scene): (target.position[0], target.position[1], target.position[2] - 0.057 )) with pytest.raises(InverseKinematicsError): - skill._solve_goal_ik( # pylint: disable=protected-access + skill._solve_goal_ik_candidates( # pylint: disable=protected-access fake, target, [0.5] * 7, validate=False) From acdd65bbf85ba85ef5b38d3ee004858658fe6b18 Mon Sep 17 00:00:00 2001 From: Yichao Liang Date: Wed, 19 Aug 2026 06:43:13 -0400 Subject: [PATCH 11/30] skills: robot-link start escape; start-local partner demotion Two BiRRT margin rules that stop start-config contacts from poisoning whole plans: 1. Robot links can begin a phase already in modeled contact (a finger or wrist link 5-15 mm inside the object it just grasped or settled on, from execution-side sag and settle). Each such body gets a per-body escape allowance -- the start depth minus 3 mm slack -- active only within 0.5 rad of the start config, so the arm may pull out of a contact it began in but can never deepen it or re-enter it later in the path. 2. Contact-partner status earned SOLELY at the start config no longer licenses hard-margin penetration for the whole path. A movable body the robot merely starts near keeps the hard margin only inside the start radius and gets a no-penetration margin beyond it (touching stays legal). Static bodies keep their partner margin throughout -- they cannot be shoved -- and so do goal-earned partners. Rationale: the hard margin tolerates enough penetration to shove a free-standing object, and a retreat after a glue dab repeatedly nudged an assembled row it had grazed on the way out. Tests: a thin-wall scene asserts no penetration beyond the start radius across seeds while a static wall still plans; a robot start-escape test pins the per-body allowance contract. --- .../pybullet_helpers/motion_planning.py | 112 +++++++++- .../pybullet_helpers/test_motion_planning.py | 194 ++++++++++++++++++ 2 files changed, 298 insertions(+), 8 deletions(-) diff --git a/predicators/pybullet_helpers/motion_planning.py b/predicators/pybullet_helpers/motion_planning.py index cddddda74..f3fac91cd 100644 --- a/predicators/pybullet_helpers/motion_planning.py +++ b/predicators/pybullet_helpers/motion_planning.py @@ -15,6 +15,23 @@ from predicators.pybullet_helpers.robots import SingleArmPyBulletRobot from predicators.settings import CFG +# Escape allowance for ROBOT links already in modeled contact at the +# start configuration (see run_motion_planning): near the start, the +# path may keep such a contact, but never more than this much deeper +# than it began (meters). +_START_ESCAPE_DEPTH_SLACK = 0.003 +# ... and only while within this max-abs joint distance of the start +# configuration (radians for revolute joints). Beyond it -- and at any +# goal further away -- full margins apply, so the allowance cannot be +# exploited elsewhere on the path. The same radius bounds how far a +# body's start-earned contact-partner status carries; see +# run_motion_planning. +_START_LOCAL_JOINT_RADIUS = 0.5 +# Margin applied to a MOVABLE body whose contact-partner status was +# earned only at the start configuration, once the path has left that +# start neighborhood: touching stays legal, penetration does not. +_DEMOTED_PARTNER_MARGIN = 0.0 + def run_motion_planning( robot: SingleArmPyBulletRobot, @@ -40,6 +57,19 @@ def run_motion_planning( bystanders from which the path must keep ``CFG.pybullet_birrt_bystander_clearance`` of separation. + Partner status earned SOLELY at the start configuration is local to + it: a movable body the robot merely happens to begin near is checked + with the hard margin only while the path stays within a joint-space + radius of the start, and with a no-penetration margin beyond it. + Contact the start forces on us says nothing about what the path may + do to that body half a metre later, and the hard margin tolerates + enough penetration to shove a free-standing object (a retreat after + a glue dab repeatedly nudged an assembled row it had grazed on the + way out). Static bodies keep their partner margin throughout -- they + cannot be shoved, so grazing them is a modeling artifact rather than + a physical event -- and goal-earned partners keep it too, since the + path is deliberately approaching them. + ``held_attachments`` maps bodies rigidly attached to the held object (e.g. the welded members of a glued assembly) to their base-link-relative transforms, in the same ``(position, orientation)`` @@ -76,6 +106,18 @@ def run_motion_planning( static support is always safe, whereas deep start penetration into a movable body still signals genuine trouble and keeps the margin. + ROBOT links get an analogous (always-on) start-escape allowance: a + robot-vs-body contact already present at the start configuration + and no deeper than the shallow margin does not reject the path near + the start, as long as it never deepens beyond how it began (plus a + small slack) and the configuration stays within a joint-space + radius of the start. The planning scene is reconstructed from + observable features, so a phase that begins right after a grasp or + a settled place can model a finger or wrist link several mm inside + the object it just touched; the start is a fact, and escaping from + it is strictly better than the guaranteed option failure that + rejecting it produces. + Note that this function changes the state of the robot. """ rng = np.random.default_rng(seed) @@ -157,6 +199,35 @@ def _set_state(pt: JointPositions) -> None: elif start_depth >= shallow_margin: allowed_shallow_held_margins[body] = shallow_margin + # Robot links, like the held assembly, can begin a phase already in + # modeled contact: the planning scene is reconstructed from + # observable features, and right after a grasp or a settled place + # that reconstruction can show a finger or wrist link 5-15 mm + # inside the object it just touched (execution-side sag and settle + # are not in the feature model). The start configuration is a fact, + # not a choice -- rejecting it fails the whole option with + # certainty -- so a start contact no deeper than the shallow margin + # gets a per-body escape allowance: near the start the path may + # keep that contact, never more than _START_ESCAPE_DEPTH_SLACK + # deeper than it began, and only within + # _START_LOCAL_JOINT_RADIUS of the start configuration. Deeper + # start penetration still signals genuine scene corruption and + # keeps the hard rejection. + allowed_robot_escape_margins: Dict[int, float] = {} + _set_state(initial_positions) + p.performCollisionDetection(physicsClientId=physics_client_id) + for body in collision_bodies: + contacts = p.getContactPoints(robot.robot_id, + body, + physicsClientId=physics_client_id) + depths = [c[8] for c in contacts if c[8] < hard_margin] + if not depths: + continue + start_depth = min(depths) + if start_depth >= shallow_margin: + allowed_robot_escape_margins[body] = \ + start_depth - _START_ESCAPE_DEPTH_SLACK + # Bodies the robot or held object starts or deliberately ends within # the clearance of are intended contact partners (support surfaces, # grasp targets, placement neighbors) and keep the hard margin; @@ -180,19 +251,26 @@ def _set_state(pt: JointPositions) -> None: else CFG.pybullet_birrt_held_bystander_clearance held_body_clearances: dict = {} contact_partners: set = set(collision_bodies) + # Movable bodies that earned partner status only at the start (see + # the docstring): their hard margin expires with the start + # neighborhood. + demoted_partners: set = set() if bystander_clearance > hard_margin: contact_partners = set() + endpoint_partners: List[set] = [set(), set()] held_probe_radius = max(bystander_clearance, held_clearance) held_near_endpoint: set = set() - for pt in (initial_positions, target_positions): + for endpoint_idx, pt in enumerate( + (initial_positions, target_positions)): _set_state(pt) for body in collision_bodies: - if body not in contact_partners: - if p.getClosestPoints(robot.robot_id, - body, - bystander_clearance, - physicsClientId=physics_client_id): - contact_partners.add(body) + if p.getClosestPoints(robot.robot_id, + body, + bystander_clearance, + physicsClientId=physics_client_id): + contact_partners.add(body) + endpoint_partners[endpoint_idx].add(body) + continue # Evaluate held proximity at BOTH endpoints, even for a # body already seen near the other one: partner status # (within the clearance) at EITHER endpoint must win. @@ -214,7 +292,14 @@ def _set_state(pt: JointPositions) -> None: if held_dists: if min(held_dists) < bystander_clearance: contact_partners.add(body) + endpoint_partners[endpoint_idx].add(body) held_near_endpoint.add(body) + for body in endpoint_partners[0] - endpoint_partners[1]: + # Base mass 0 marks a static body (table, wall): nothing the + # path does can displace it, so its partner margin stands. + if p.getDynamicsInfo(body, -1, + physicsClientId=physics_client_id)[0] > 0: + demoted_partners.add(body) if held_assembly and held_clearance > bystander_clearance: held_body_clearances = { body: held_clearance @@ -244,13 +329,24 @@ def _collision_fn(pt: JointPositions) -> bool: # clearance (Bullet generates contact points out to its # contactBreakingThreshold, 0.02 by default, so millimetre-scale # positive distances are reported here). + near_start = True + if allowed_robot_escape_margins or demoted_partners: + near_start = float( + np.max(np.abs(np.subtract( + pt, initial_positions)))) < _START_LOCAL_JOINT_RADIUS + robot_escape_active = bool(allowed_robot_escape_margins) and near_start for body in collision_bodies: margin = hard_margin if body in contact_partners \ else bystander_clearance + if not near_start and body in demoted_partners: + margin = _DEMOTED_PARTNER_MARGIN + robot_margin = margin + if robot_escape_active and body in allowed_robot_escape_margins: + robot_margin = allowed_robot_escape_margins[body] contacts = p.getContactPoints(robot.robot_id, body, physicsClientId=physics_client_id) - if any(c[8] < margin for c in contacts): + if any(c[8] < robot_margin for c in contacts): return True for assembly_body, _ in held_assembly: # Clearances above Bullet's contactBreakingThreshold diff --git a/tests/pybullet_helpers/test_motion_planning.py b/tests/pybullet_helpers/test_motion_planning.py index 1620f3f16..5cd47c3c3 100644 --- a/tests/pybullet_helpers/test_motion_planning.py +++ b/tests/pybullet_helpers/test_motion_planning.py @@ -188,6 +188,200 @@ def test_bystander_clearance(physics_client_id): p.removeBody(block_id, physicsClientId=physics_client_id) +def test_robot_start_escape(physics_client_id): + """A start config with a shallow robot-vs-body contact still plans. + + The planning scene is reconstructed from observable features, so a + phase that begins right after a grasp or a settled place can model + a finger or wrist link several mm inside the object it just + touched. Such a start is a fact, not a choice: it must not reject + the whole plan; the path escapes the contact instead (never going + deeper than it began). Start penetration deeper than the shallow + margin still rejects. + """ + utils.reset_config({ + "pybullet_birrt_contact_margin": -0.001, + "pybullet_birrt_shallow_held_contact_margin": -0.02, + }) + ee_home_position = (1.35, 0.75, 0.75) + ee_orn = p.getQuaternionFromEuler([0.0, np.pi / 2, -np.pi]) + ee_home_pose = Pose(ee_home_position, ee_orn) + robot = create_single_arm_pybullet_robot("fetch", physics_client_id, + ee_home_pose) + robot_init_state = tuple(ee_home_position) + tuple( + ee_orn, ) + (robot.open_fingers, ) + robot.reset_state(robot_init_state) + joint_initial = robot.get_joints() + block_id = create_pybullet_block(color=(0.0, 0.0, 1.0, 1.0), + half_extents=(0.03, 0.03, 0.03), + mass=0, + friction=1, + orientation=[0., 0., 0., 1.], + physics_client_id=physics_client_id) + + def _min_robot_dist(z: float) -> float: + p.resetBasePositionAndOrientation(block_id, (1.35, 0.75, z), + [0., 0., 0., 1.], + physicsClientId=physics_client_id) + robot.set_joints(joint_initial) + p.performCollisionDetection(physicsClientId=physics_client_id) + contacts = p.getContactPoints(robot.robot_id, + block_id, + physicsClientId=physics_client_id) + return min((c[8] for c in contacts), default=float("inf")) + + # Raise the block toward the gripper until a robot link is modeled + # 5-12 mm inside it (the artifact depth seen in post-grasp / + # post-place reconstructions). + shallow_z = None + for z in np.arange(0.40, 0.80, 0.001): + depth = _min_robot_dist(z) + if -0.012 < depth < -0.005: + shallow_z = z + break + if depth <= -0.012: + break + assert shallow_z is not None + start_depth = _min_robot_dist(shallow_z) + ee_target = Pose((1.35, 0.75, 0.90), ee_orn) + joint_target = robot.inverse_kinematics(ee_target, validate=True) + path = None + # Motion planning is non-deterministic (RRT); try multiple seeds. + for seed in [123, 456, 789]: + robot.set_joints(joint_initial) + path = run_motion_planning(robot, + joint_initial, + joint_target, + collision_bodies={block_id}, + seed=seed, + physics_client_id=physics_client_id) + if path is not None: + break + assert path is not None + # The escape never deepens the start contact beyond how it began + # (plus the small slack), and the goal keeps the hard margin. + for pt in path: + robot.set_joints(pt) + p.performCollisionDetection(physicsClientId=physics_client_id) + contacts = p.getContactPoints(robot.robot_id, + block_id, + physicsClientId=physics_client_id) + assert all(c[8] >= start_depth - 0.003 - 1e-6 for c in contacts) + robot.set_joints(path[-1]) + p.performCollisionDetection(physicsClientId=physics_client_id) + contacts = p.getContactPoints(robot.robot_id, + block_id, + physicsClientId=physics_client_id) + assert all(c[8] >= -0.001 for c in contacts) + # Start penetration deeper than the shallow margin still signals + # genuine scene corruption and rejects the plan. + deep_z = None + for z in np.arange(shallow_z, 0.90, 0.002): + if _min_robot_dist(z) < -0.025: + deep_z = z + break + assert deep_z is not None + robot.set_joints(joint_initial) + path = run_motion_planning(robot, + joint_initial, + joint_target, + collision_bodies={block_id}, + seed=123, + physics_client_id=physics_client_id) + assert path is None + p.removeBody(block_id, physicsClientId=physics_client_id) + + +def test_start_local_partner_demotion(physics_client_id): + """Partner status earned only at the start expires with the start. + + A movable body the robot merely begins near is checked with the + hard contact margin only inside the start neighborhood; beyond it + the path may touch the body but not penetrate it. Otherwise a body + grazed on the way out of the start keeps a penetration allowance + for the entire path, which physically shoves it (a bottle retreat + after a glue dab repeatedly nudged an assembled row this way). + Static bodies cannot be shoved and keep their partner margin. + """ + utils.reset_config({ + "pybullet_birrt_contact_margin": -0.03, + "pybullet_birrt_bystander_clearance": 0.005, + }) + ee_home_position = (1.35, 0.75, 0.75) + ee_orn = p.getQuaternionFromEuler([0.0, np.pi / 2, -np.pi]) + ee_home_pose = Pose(ee_home_position, ee_orn) + robot = create_single_arm_pybullet_robot("fetch", physics_client_id, + ee_home_pose) + robot.reset_state( + tuple(ee_home_position) + tuple(ee_orn, ) + (robot.open_fingers, )) + joint_initial = robot.get_joints() + # The goal is on the far side of a thin wall, so the path must + # travel around it: plenty of opportunity to graze it mid-flight. + ee_target = Pose((1.35, 0.4, 0.6), ee_orn) + joint_target = robot.inverse_kinematics(ee_target, validate=True) + assert np.max(np.abs(np.subtract(joint_target, joint_initial))) > 0.5 + + def _plan_around_wall(mass: float, seed: int): + wall_id = create_pybullet_block(color=(1.0, 0.0, 0.0, 1.0), + half_extents=(0.2, 0.01, 0.3), + mass=mass, + friction=1, + orientation=[0., 0., 0., 1.], + physics_client_id=physics_client_id) + # Slide the wall toward the arm until the start config is just + # within the bystander clearance of it (earning partner status) + # without penetrating it. + near_start = False + for wall_y in np.arange(0.80, 0.55, -0.002): + p.resetBasePositionAndOrientation( + wall_id, (1.35, wall_y, 0.5), [0., 0., 0., 1.], + physicsClientId=physics_client_id) + robot.set_joints(joint_initial) + contacts = p.getClosestPoints(robot.robot_id, + wall_id, + 0.005, + physicsClientId=physics_client_id) + distances = [c[8] for c in contacts] + if distances and min(distances) > 0.0: + near_start = True + break + assert near_start + robot.set_joints(joint_initial) + return wall_id, run_motion_planning( + robot, + joint_initial, + joint_target, + collision_bodies={wall_id}, + seed=seed, + physics_client_id=physics_client_id) + + for seed in [123, 456, 789]: + wall_id, path = _plan_around_wall(1.0, seed) + assert path is not None + num_far = 0 + for pt in path: + if np.max(np.abs(np.subtract(pt, joint_initial))) < 0.5: + continue + num_far += 1 + robot.set_joints(pt) + p.performCollisionDetection(physicsClientId=physics_client_id) + contacts = p.getContactPoints(robot.robot_id, + wall_id, + physicsClientId=physics_client_id) + # Beyond the start neighborhood, touching is still legal but + # penetrating the movable wall is not. + assert all(c[8] >= -1e-6 for c in contacts) + assert num_far > 0 + p.removeBody(wall_id, physicsClientId=physics_client_id) + # The same wall, static: nothing the path does can displace it, so + # its partner margin stands for the whole path and planning is not + # made harder. (With this geometry the undemoted margin is real: + # seed 123 routes a link 17 mm through the static wall.) + wall_id, path = _plan_around_wall(0.0, 123) + assert path is not None + p.removeBody(wall_id, physicsClientId=physics_client_id) + + def test_held_attachments(physics_client_id): """Bodies rigidly attached to the held object are collision-checked. From d1bbd0434a7371494089ca22a2b21cd9650a1cbf Mon Sep 17 00:00:00 2001 From: Yichao Liang Date: Wed, 19 Aug 2026 06:43:24 -0400 Subject: [PATCH 12/30] skills: escalated goal-IK on goal collisions; proximity-ordered branches When BiRRT fails and diagnostics show the GOAL config in collision, re-solve goal IK with 32 restarts instead of 8 and re-plan: the pose is often reachable by another arm branch the small restart budget missed. If none of the escalated branches is collision-free either, the diagnostics now say so explicitly -- the pose itself sits in clutter, and no path or arm branch can fix it. Goal-IK candidates are returned proximity-first: the priority seeds (current joints, home) lead, and restart-derived branches sort by max-abs joint distance from the current config, so planning tries the least contorted branch that reaches the pose before the exotic ones. _log_collision_diagnostics gains log_errors so the escalation path can inspect diagnostics without spamming the error log; a final failure still logs them once. --- .../skill_factories/base.py | 205 ++++++++++++------ tests/test_skill_factories.py | 114 ++++++++++ 2 files changed, 251 insertions(+), 68 deletions(-) diff --git a/predicators/ground_truth_models/skill_factories/base.py b/predicators/ground_truth_models/skill_factories/base.py index f693ed959..0a0566a69 100644 --- a/predicators/ground_truth_models/skill_factories/base.py +++ b/predicators/ground_truth_models/skill_factories/base.py @@ -273,6 +273,10 @@ def _fmt_option_params(params: Array) -> str: # execution. _RELEASE_CLEAR_SLACK = 0.008 _RELEASE_CHECK_BUFFER = _RELEASE_OPEN_STEP + _RELEASE_CLEAR_SLACK + 0.002 +# Goal-IK seeds tried before the random restarts (current joints, then +# home); their candidates lead the returned branch order, see +# PhaseSkill._solve_goal_ik_candidates. +_NUM_PRIORITY_GOAL_IK_SEEDS = 2 _IK_STALL_BEST_KEY = "ik_stall_best_{}" # best EE-to-target distance seen _DWELL_COUNT_KEY = "dwell_count_{}" # post-terminal hold steps taken _STROKE_BEST_KEY = "stroke_best_{}" # gentle stroke: best EE distance @@ -888,6 +892,11 @@ def _execute_gentle_stroke(self, phase: Phase, state: State, memory: Dict, # Random in-limit IK restarts for the BiRRT goal solve, tried after # the current-joints and home seeds (see _solve_goal_ik). _goal_ik_num_restarts: ClassVar[int] = 8 + # Escalated restart count for the goal solve, used when every branch + # the normal solve found puts the goal configuration in collision + # (see _plan_with_simulator). A grasp pose in clutter can need an + # arm branch that eight restarts never sample. + _goal_ik_escalated_num_restarts: ClassVar[int] = 32 _ik_stall_min_progress: ClassVar[float] = 2e-3 # meters def _maybe_drive_base(self, phase: Phase, state: State, memory: Dict, @@ -1456,86 +1465,118 @@ def _plan_with_simulator( else: goal_finger_joint = self._config.open_fingers_joint - traj = run_motion_planning( - robot=planning_robot, - initial_positions=pb_state.joint_positions, - target_positions=target_joints, - collision_bodies=collision_bodies, - seed=CFG.seed, - physics_client_id=sim._physics_client_id, # pylint: disable=protected-access - held_object=held_object, - base_link_to_held_obj=base_link_to_held_obj, - held_attachments=held_attachments, - allow_shallow_held_object_contacts=( - phase.allow_shallow_held_object_contacts - if phase is not None else False), - unbounded_shallow_bodies=self._sim_table_ids(sim), - goal_finger_joint=goal_finger_joint, - held_bystander_clearance=self._config.held_bystander_clearance, - goal_candidates=goal_candidates, - ) + def _plan( + candidates: List[JointPositions] + ) -> Optional[Sequence[JointPositions]]: + return run_motion_planning( + robot=planning_robot, + initial_positions=pb_state.joint_positions, + target_positions=candidates[0], + collision_bodies=collision_bodies, + seed=CFG.seed, + physics_client_id=sim._physics_client_id, # pylint: disable=protected-access + held_object=held_object, + base_link_to_held_obj=base_link_to_held_obj, + held_attachments=held_attachments, + allow_shallow_held_object_contacts=( + phase.allow_shallow_held_object_contacts + if phase is not None else False), + unbounded_shallow_bodies=self._sim_table_ids(sim), + goal_finger_joint=goal_finger_joint, + held_bystander_clearance=( + self._config.held_bystander_clearance), + goal_candidates=candidates, + ) - if traj is None and not validate_goal_ik: - # The unvalidated goal solve may have accepted a one-shot IK - # branch whose carried object is in collision. Before declaring - # the option infeasible, retry with the fully validated goal-IK - # stack (same restart machinery), which can land a different - # in-limit branch whose goal configuration is collision-free. + def _resolve_goal_ik( + num_restarts: Optional[int] = None + ) -> Optional[List[JointPositions]]: + """Re-solve the goal IK from a clean scene, or None if it fails.""" sim._set_state(remapped_state) # pylint: disable=protected-access planning_robot.set_joints(pb_state.joint_positions) - validated_candidates: Optional[List[JointPositions]] = None try: - validated_candidates = self._solve_goal_ik_candidates( + return self._solve_goal_ik_candidates( planning_robot, target_pose, pb_state.joint_positions, - validate=True) + validate=True, + num_restarts=num_restarts) except InverseKinematicsError: - pass - if validated_candidates is not None and \ - validated_candidates != goal_candidates: - traj = run_motion_planning( - robot=planning_robot, - initial_positions=pb_state.joint_positions, - target_positions=validated_candidates[0], - collision_bodies=collision_bodies, - seed=CFG.seed, - physics_client_id=sim._physics_client_id, # pylint: disable=protected-access - held_object=held_object, - base_link_to_held_obj=base_link_to_held_obj, - held_attachments=held_attachments, - allow_shallow_held_object_contacts=( - phase.allow_shallow_held_object_contacts - if phase is not None else False), - unbounded_shallow_bodies=self._sim_table_ids(sim), - goal_finger_joint=goal_finger_joint, - held_bystander_clearance=( - self._config.held_bystander_clearance), - goal_candidates=validated_candidates, - ) - if traj is not None: - target_joints = validated_candidates[0] + return None - if traj is None and not expect_contact: - self._last_plan_diagnostics = self._log_collision_diagnostics( + def _diagnose(goal_joints: JointPositions, + log_errors: bool) -> List[str]: + return self._log_collision_diagnostics( planning_robot, sim._physics_client_id, # pylint: disable=protected-access pb_state.joint_positions, - target_joints, + goal_joints, collision_bodies, held_object, base_link_to_held_obj, phase_name, body_names=body_names, goal_finger_joint=goal_finger_joint, - held_attachments=held_attachments) + held_attachments=held_attachments, + log_errors=log_errors) + + traj = _plan(goal_candidates) + + if traj is None and not validate_goal_ik: + # The unvalidated goal solve may have accepted a one-shot IK + # branch whose carried object is in collision. Before declaring + # the option infeasible, retry with the fully validated goal-IK + # stack (same restart machinery), which can land a different + # in-limit branch whose goal configuration is collision-free. + validated_candidates = _resolve_goal_ik() + if validated_candidates is not None and \ + validated_candidates != goal_candidates: + traj = _plan(validated_candidates) + if traj is not None: + target_joints = validated_candidates[0] + goal_candidates = validated_candidates + + diagnostics: List[str] = [] + if traj is None and not expect_contact: + diagnostics = _diagnose(target_joints, log_errors=False) + if any(d.startswith("GOAL") for d in diagnostics): + # The goal CONFIGURATION, not the path, is what failed -- + # and unlike the start, the goal configuration is a + # choice: the same end-effector pose is reachable by + # several arm branches, which are pose-equivalent but not + # collision-equivalent. When every branch the normal + # solve sampled sits in clutter (a grasp goal 15 mm + # inside a leg, with the target object parked beside it), + # sample many more branches before calling the pose + # infeasible. + escalated = _resolve_goal_ik( + num_restarts=self._goal_ik_escalated_num_restarts) + if escalated is not None and escalated != goal_candidates: + traj = _plan(escalated) + if traj is not None: + target_joints = escalated[0] + else: + diagnostics = _diagnose(escalated[0], log_errors=False) + diagnostics.append( + f"GOAL: none of the {len(escalated)} distinct " + "arm configurations that reach this pose is " + "collision-free, so the pose itself sits in " + "clutter (no path or arm branch can fix it)") + + if traj is None and not expect_contact: + for diag in diagnostics: + logging.error("[%s/%s] %s", self._name, phase_name, diag) + self._last_plan_diagnostics = diagnostics return traj - def _solve_goal_ik_candidates(self, planning_robot: SingleArmPyBulletRobot, - target_pose: Pose, - current_joints: JointPositions, - validate: bool) -> List[JointPositions]: + def _solve_goal_ik_candidates( + self, + planning_robot: SingleArmPyBulletRobot, + target_pose: Pose, + current_joints: JointPositions, + validate: bool, + num_restarts: Optional[int] = None) -> List[JointPositions]: """All distinct pose-accurate goal configs, ordered by seed priority. PyBullet IK is a one-shot approximation with no accuracy @@ -1554,16 +1595,28 @@ def _solve_goal_ik_candidates(self, planning_robot: SingleArmPyBulletRobot, Seeds: the current joints, the home configuration, then deterministic random in-limit restarts. - ALL accepted candidates are returned (deduplicated, seed order - preserved, so the current-joints branch comes first): which arm + ALL accepted candidates are returned (deduplicated): which arm BRANCH a single solve lands on is seed-dependent, and branches are pose-equivalent but not collision-equivalent -- one grasp branch can sweep a link 6 cm through a neighboring block while another clears it. ``run_motion_planning`` picks the first collision-free candidate (see its ``goal_candidates``), turning - that per-seed coin flip into a deterministic choice. Raise + that per-seed coin flip into a deterministic choice. + + Because that pick is first-past-the-post, ORDER is a safety + property, not a formality. The two priority seeds (current + joints, then home) lead, and the random restarts follow sorted + by joint distance from the current configuration, so the chosen + branch is the least contorted one that clears. A far branch + reaches the same end-effector pose by swinging the whole arm + through the scene: legal to within the contact margin, but it + grazes what it passes and leaves the following phases starting + from an awkward configuration. Unsorted, a wider restart pool + makes that outcome MORE likely, exactly when the pool was + widened because the scene is cluttered. Raise ``InverseKinematicsError`` when no seed produces an acceptable - config. + config. ``num_restarts`` overrides ``_goal_ik_num_restarts`` + (the caller escalates it when every branch found collides). """ limits = list( zip(planning_robot.joint_lower_limits, @@ -1573,7 +1626,9 @@ def _solve_goal_ik_candidates(self, planning_robot: SingleArmPyBulletRobot, list(planning_robot.initial_joint_positions), ] rng = np.random.default_rng(CFG.seed) - for _ in range(self._goal_ik_num_restarts): + if num_restarts is None: + num_restarts = self._goal_ik_num_restarts + for _ in range(num_restarts): seeds.append([ float(rng.uniform(lo, hi)) if np.isfinite(lo) and np.isfinite(hi) and lo <= hi else float( @@ -1581,8 +1636,11 @@ def _solve_goal_ik_candidates(self, planning_robot: SingleArmPyBulletRobot, for (lo, hi), cur in zip(limits, current_joints) ]) candidates: List[JointPositions] = [] + # Candidates from the two priority seeds keep the lead; the + # restart-derived ones are ordered by proximity below. + num_priority_candidates = 0 best_err = float("inf") - for seed in seeds: + for seed_idx, seed in enumerate(seeds): for attempt_validate in ((True, ) if validate else (False, True)): planning_robot.set_joints(seed) try: @@ -1608,6 +1666,8 @@ def _solve_goal_ik_candidates(self, planning_robot: SingleArmPyBulletRobot, for a, b in zip(clamped, prior)) < 1e-3 for prior in candidates): candidates.append(clamped) + if seed_idx < _NUM_PRIORITY_GOAL_IK_SEEDS: + num_priority_candidates += 1 break best_err = min(best_err, err) if not candidates: @@ -1615,7 +1675,10 @@ def _solve_goal_ik_candidates(self, planning_robot: SingleArmPyBulletRobot, f"Goal IK missed the target pose from all {len(seeds)} seeds " f"(best squared FK error after limit clamping {best_err:.6f})." ) - return candidates + return candidates[:num_priority_candidates] + sorted( + candidates[num_priority_candidates:], + key=lambda cand: max( + abs(a - b) for a, b in zip(cand, current_joints))) def _log_collision_diagnostics( self, @@ -1630,6 +1693,7 @@ def _log_collision_diagnostics( body_names: Optional[Dict[int, str]] = None, goal_finger_joint: Optional[float] = None, held_attachments: Optional[Dict[int, Any]] = None, + log_errors: bool = True, ) -> List[str]: """Log which collision bodies cause start/goal collisions. @@ -1637,6 +1701,10 @@ def _log_collision_diagnostics( ``OptionExecutionFailure`` - in the agent's sandbox that message is the only channel through which it learns WHICH object blocked the motion plan (and hence how to adjust its target pose). + + ``log_errors=False`` computes the same strings quietly, for + callers that inspect them before deciding whether the failure is + final (see the goal-branch escalation in _plan_with_simulator). """ diagnostics: List[str] = [] @@ -1713,8 +1781,9 @@ def _check(joints: JointPositions, label: str) -> None: release_joints, "GOAL with fingers OPEN to release (the opening " "gripper needs side clearance at the drop pose)") - for diag in diagnostics: - logging.error("[%s/%s] %s", self._name, phase_name, diag) + if log_errors: + for diag in diagnostics: + logging.error("[%s/%s] %s", self._name, phase_name, diag) return diagnostics # Gentle strokes (Phase.max_step_norm): any single arm joint asked to diff --git a/tests/test_skill_factories.py b/tests/test_skill_factories.py index 0080361f9..db454b068 100644 --- a/tests/test_skill_factories.py +++ b/tests/test_skill_factories.py @@ -4,6 +4,8 @@ make_move_to_phase, create_move_to_skill, create_pick_skill, create_place_skill, create_push_skill. """ +import logging + import numpy as np import pybullet as p import pytest @@ -26,6 +28,7 @@ from predicators.pybullet_helpers.geometry import Pose from predicators.pybullet_helpers.inverse_kinematics import \ InverseKinematicsError +from predicators.pybullet_helpers.objects import create_pybullet_block from predicators.pybullet_helpers.robots import \ create_single_arm_pybullet_robot from predicators.structs import Action, Object, ParameterizedOption, Type @@ -1448,3 +1451,114 @@ def test_all_branches_inaccurate_raises(self, robot_scene): fake, target, [0.5] * 7, validate=False) + + def test_escalated_restarts_extend_the_branch_set(self, robot_scene): + """More restarts only ever ADD branches, in the same order. + + When every branch of the normal solve puts the goal + configuration in collision, ``_plan_with_simulator`` re-solves + with ``_goal_ik_escalated_num_restarts``. That is only worth + doing if the escalated solve is a superset of the normal one: + the deterministic seed prefix is unchanged, so a branch that + already failed is not re-planned, and only genuinely new + configurations are tried. + """ + _, robot = robot_scene + skill = self._make_skill(robot) + target = Pose((0.77, 1.34, 0.55)) + escalated_restarts = PhaseSkill._goal_ik_escalated_num_restarts # pylint: disable=protected-access + + def _make_fake(): + fake = _FakeGoalIkRobot(target, one_shot_error_m=0.0) + branches = [[0.01 * i] * 7 for i in range(2 + escalated_restarts)] + fake.inverse_kinematics = ( # type: ignore + lambda *a, _it=iter(branches), **k: next(_it)) + fake.forward_kinematics = ( # type: ignore + lambda joints: Pose(target.position)) + return fake + + default = skill._solve_goal_ik_candidates( # pylint: disable=protected-access + _make_fake(), + target, [0.5] * 7, + validate=True) + escalated = skill._solve_goal_ik_candidates( # pylint: disable=protected-access + _make_fake(), + target, [0.5] * 7, + validate=True, + num_restarts=escalated_restarts) + assert len(default) == _GOAL_IK_NUM_SEEDS + assert len(escalated) == 2 + escalated_restarts + # The two priority seeds still lead, and every branch the normal + # solve found is still offered; the restart-derived ones are + # ordered by proximity to the current configuration, so a wider + # pool cannot push a NEARER branch behind a contorted one. + assert escalated[:2] == default[:2] + assert all(cand in escalated for cand in default) + current = [0.5] * 7 + spread = [ + max(abs(a - b) for a, b in zip(cand, current)) + for cand in escalated[2:] + ] + assert spread == sorted(spread) + + +class TestCollisionDiagnosticsLogging: + """The diagnostics can be computed quietly, before the failure is + known to be final.""" + + def _make_skill(self, robot) -> PhaseSkill: + config = _make_config(robot) + phase = Phase( + name="MoveToGrasp", + action_type=PhaseAction.MOVE_TO_POSE, + target_fn=lambda *args: None, + use_motion_planning=True, + ) + return PhaseSkill("Pick", [_ROBOT_TYPE], Box(0, 1, (0, )), config, + [phase]) + + def test_log_errors_false_reports_without_logging(self, robot_scene, + caplog): + """``log_errors=False`` returns the same strings but logs nothing. + + ``_plan_with_simulator`` inspects the diagnostics to decide + whether to escalate the goal-IK branch search; escalating can + turn the failure into a success, so the ERROR lines must not be + emitted until the failure is final. + """ + physics_client_id, robot = robot_scene + skill = self._make_skill(robot) + robot.reset_state( + tuple(_EE_HOME) + tuple(_get_ee_home_pose().orientation) + + (robot.open_fingers, )) + joints = robot.get_joints() + # A block straddling the gripper guarantees START contacts. + block_id = create_pybullet_block(color=(1.0, 0.0, 0.0, 1.0), + half_extents=(0.05, 0.05, 0.05), + mass=0, + friction=1, + orientation=[0., 0., 0., 1.], + physics_client_id=physics_client_id) + p.resetBasePositionAndOrientation(block_id, + _EE_HOME, [0., 0., 0., 1.], + physicsClientId=physics_client_id) + try: + with caplog.at_level(logging.ERROR): + quiet = skill._log_collision_diagnostics( # pylint: disable=protected-access + robot, + physics_client_id, + joints, + joints, {block_id}, + None, + None, + "MoveToGrasp", + log_errors=False) + assert quiet + assert not caplog.records + loud = skill._log_collision_diagnostics( # pylint: disable=protected-access + robot, physics_client_id, joints, joints, {block_id}, None, + None, "MoveToGrasp") + assert loud == quiet + assert len(caplog.records) == len(loud) + finally: + p.removeBody(block_id, physicsClientId=physics_client_id) From de55f579c1ec7087a30fef7c73197019b88fea79 Mon Sep 17 00:00:00 2001 From: Yichao Liang Date: Wed, 19 Aug 2026 06:43:24 -0400 Subject: [PATCH 13/30] bridge: tack wet joints until they weld A wet glue joint was held only by friction while curing: a release impulse or a neighbouring operation could shear the mated pair apart mm-scale during the cure dwell, and the eventual weld froze that drift in. Every wet, mated joint now carries a weak JOINT_FIXED tack (0.5 N max force, below a block's ~1 N weight, so a tack can never lift or drag its neighbour) at the current relative pose, replaced by the rigid weld at latch. _latch_joint now returns bool so a refused latch keeps its tack instead of silently leaving the joint loose, and _sync_welds_to_state tears tacks down before rebuilding welds. Tests: a welded-pair-plus-newcomer stage drifts 1.98 mm under a release impulse with the tack off and 0.03 mm with it on; a second test pins the tack lifecycle from wetting to weld. --- predicators/envs/pybullet_bridge.py | 94 +++++++++++++++++++++++-- tests/envs/test_pybullet_bridge.py | 102 ++++++++++++++++++++++++++++ 2 files changed, 190 insertions(+), 6 deletions(-) diff --git a/predicators/envs/pybullet_bridge.py b/predicators/envs/pybullet_bridge.py index 56f84bbce..ca7d65dbd 100644 --- a/predicators/envs/pybullet_bridge.py +++ b/predicators/envs/pybullet_bridge.py @@ -398,6 +398,9 @@ def __init__(self, use_gui: bool = False, **kwargs: Any) -> None: # a resting weld can be re-anchored (see _relax_resting_welds). self._weld_meta: Dict[FrozenSet[int], Tuple[int, int, Optional[float]]] = {} + # Live wet-glue tacks (see _sync_wet_joint_tacks): + # frozenset({body_id_a, body_id_b}) -> constraint id. + self._tack_constraints: Dict[FrozenSet[int], int] = {} # Glue-patch visual bodies: block name -> face -> body id. self._glue_patch_ids: Dict[str, Dict[str, int]] = {} @@ -1045,6 +1048,68 @@ def _gap(ideal: float, ref: float = actual_dz) -> float: pairs[key] = (blk.id, partner.id, dz) return pairs + # Wet glue is tacky: while a joint is wet and its faces are in + # aligned contact, the pair is held together by a weak constraint + # (under a newton, against the weld's ten thousand). It does not + # stop the impulse the arm leaves behind when it releases and + # retreats -- momentum is momentum -- but it makes the joint absorb + # that impulse as a UNIT instead of coming apart: a placement that + # ended flush against its neighbor was observed to fling an + # already-placed span ~5 cm and ~90 degrees during the cure wait + # (~30% of flush placements), which forced agents onto a narrow + # 3-8 mm assembly gap, wide enough to survive the release and + # narrow enough to still cure. + # + # The force is deliberately held below a block's own weight + # (block_mass * g ~ 1 N), so a wet joint can never lift, carry or + # drag its neighbour: everything the arm does deliberately still + # wins, and picking a block mid-cure aborts the cure exactly as the + # abstract model says. The tack is replaced by the rigid weld the + # moment the joint latches. + wet_joint_tack_force: ClassVar[float] = 0.5 # newtons + + def _sync_wet_joint_tacks(self, curing: Set[FrozenSet[int]]) -> None: + """Make the live tack set match the currently curing joints.""" + for key in list(self._tack_constraints): + if key not in curing: + self._drop_tack(key) + for key in curing: + if key in self._tack_constraints or key in self._weld_constraints: + continue + if self._held_obj_id is not None and self._held_obj_id in key: + continue + body_a, body_b = sorted(key) + pos_a, orn_a = p.getBasePositionAndOrientation( + body_a, physicsClientId=self._physics_client_id) + pos_b, orn_b = p.getBasePositionAndOrientation( + body_b, physicsClientId=self._physics_client_id) + inv_pos, inv_orn = p.invertTransform(pos_a, orn_a) + rel_pos, rel_orn = p.multiplyTransforms(inv_pos, inv_orn, pos_b, + orn_b) + # Anchored at the CURRENT relative pose, so the tack holds + # the joint as assembled instead of pulling it anywhere. + cid = p.createConstraint(parentBodyUniqueId=body_a, + parentLinkIndex=-1, + childBodyUniqueId=body_b, + childLinkIndex=-1, + jointType=p.JOINT_FIXED, + jointAxis=[0, 0, 0], + parentFramePosition=rel_pos, + parentFrameOrientation=rel_orn, + childFramePosition=[0, 0, 0], + childFrameOrientation=[0, 0, 0, 1], + physicsClientId=self._physics_client_id) + p.changeConstraint(cid, + maxForce=self.wet_joint_tack_force, + physicsClientId=self._physics_client_id) + self._tack_constraints[key] = cid + + def _drop_tack(self, key: FrozenSet[int]) -> None: + """Remove one wet-glue tack, if it exists.""" + cid = self._tack_constraints.pop(key, None) + if cid is not None: + p.removeConstraint(cid, physicsClientId=self._physics_client_id) + def _sync_welds_to_state(self, state: State) -> None: """Make the live constraint set match the attachment features. @@ -1053,6 +1118,10 @@ def _sync_welds_to_state(self, state: State) -> None: Persisting welds keep their original constraint (the restored poses satisfy it by construction). """ + for key in list(self._tack_constraints): + # Tacks are anchored to the poses they were created at; a + # restored state is a different scene. + self._drop_tack(key) desired = self._desired_weld_pairs(state) for key in list(self._weld_constraints): if key not in desired: @@ -1255,7 +1324,9 @@ def _domain_specific_step(self) -> None: self._set_attr(blk, f"glue_{face}", 0.0) # 2. Curing: wet faces in aligned resting contact tick; at the - # threshold the joint latches irreversibly and welds. + # threshold the joint latches irreversibly and welds. While a + # joint is merely wet it is TACKED (see _sync_wet_joint_tacks). + curing_pairs: Set[FrozenSet[int]] = set() for blk in blocks: for face in GLUE_FACES: if self._attr(blk, f"glue_{face}", 0.0) <= 0.5: @@ -1268,8 +1339,16 @@ def _domain_specific_step(self) -> None: continue cure = self._attr(blk, f"cure_{face}", 0.0) + 1.0 self._set_attr(blk, f"cure_{face}", cure) - if cure >= self.cure_threshold: - self._latch_joint(state, blk, face, mate) + assert blk.id is not None and mate.id is not None + if cure >= self.cure_threshold and \ + self._latch_joint(state, blk, face, mate): + # The rigid weld takes over from the tack. + self._drop_tack(frozenset({blk.id, mate.id})) + else: + # Still wet -- or a latch that refused (see + # _latch_joint); either way the joint stays tacked. + curing_pairs.add(frozenset({blk.id, mate.id})) + self._sync_wet_joint_tacks(curing_pairs) # 3. Anti-creep: re-anchor welds whose assembly rests free. self._relax_resting_welds() @@ -1373,15 +1452,17 @@ def _mate_slot_for(self, state: State, blk: Object, face: str, return best_slot def _latch_joint(self, state: State, blk: Object, face: str, - mate: Object) -> None: + mate: Object) -> bool: """Irreversibly attach ``blk.face`` to ``mate``: record the partnership - on both blocks, consume the glue, create the weld.""" + on both blocks, consume the glue, create the weld. + + Returns whether the joint latched.""" mate_slot = self._mate_slot_for(state, blk, face, mate) if self._attr(mate, f"attached_{mate_slot}", -1.0) >= 0: # The mate's slot is somehow taken; refuse to latch rather # than corrupt the attachment graph (cure stays at the # threshold, so this re-checks every step). - return + return False self._set_attr(blk, f"attached_{face}", float(self._block_index[mate.name])) self._set_attr(mate, f"attached_{mate_slot}", @@ -1395,6 +1476,7 @@ def _latch_joint(self, state: State, blk: Object, face: str, else: ideal_dz = 0.0 self._create_weld(blk.id, mate.id, ideal_dz=ideal_dz) + return True def _update_glue_patches(self, state: State) -> None: """Show a yellow patch on each wet face; park all other patches out of diff --git a/tests/envs/test_pybullet_bridge.py b/tests/envs/test_pybullet_bridge.py index c6136630b..d80d3a55e 100644 --- a/tests/envs/test_pybullet_bridge.py +++ b/tests/envs/test_pybullet_bridge.py @@ -10,6 +10,7 @@ from __future__ import annotations import numpy as np +import pybullet as p import pytest from predicators import utils @@ -376,3 +377,104 @@ def test_welded_pair_does_not_creep(env_and_task): assert drift < 0.002, f"{obj.name} skated {drift * 1000:.1f} mm" dyaw = abs(final.get(obj, "yaw") - latched.get(obj, "yaw")) assert dyaw < 0.01, f"{obj.name} rotated {dyaw:.4f} rad" + + +def _stage_flush_pair(env, task): + """Two spans butted flush on the table with the joint face wet. + + Returns (span0, span1) with everything else parked far away. + """ + env._set_state(task.init) + state = env._get_state() + blocks = state.get_objects(env._block_type) + span0 = next(b for b in blocks if b.name == "span0") + span1 = next(b for b in blocks if b.name == "span1") + s = state.copy() + table_z = s.get(span0, "z") + for blk, x in ((span0, 0.45), (span1, + 0.45 + 2 * env.span_half_extents[0])): + s.set(blk, "x", x) + s.set(blk, "y", 1.14) + s.set(blk, "z", table_z) + for feat in ("roll", "pitch", "yaw"): + s.set(blk, feat, 0.0) + for i, blk in enumerate(blocks): + if blk not in (span0, span1): + s.set(blk, "x", 2.0 + 0.2 * i) + s.set(blk, "y", 2.0) + env._set_state(s) + env._set_attr(span0, "glue_end_b", 1.0) + return span0, span1 + + +def test_wet_joint_is_tacked_until_it_welds(env_and_task): + """A curing joint carries a weak tack constraint, replaced by the weld.""" + env, task = env_and_task + span0, span1 = _stage_flush_pair(env, task) + key = frozenset({span0.id, span1.id}) + + env.step(_hold_action(env)) + assert set(env._tack_constraints) == {key} + assert not env._weld_constraints + + for _ in range(env.cure_threshold + 5): + env.step(_hold_action(env)) + # The latch hands the joint over to the rigid weld; no tack lingers. + assert not env._tack_constraints + assert set(env._weld_constraints) == {key} + + # Restoring any state re-derives the welds and drops stale tacks + # (they are anchored to the poses they were created at). + env._set_state(task.init) + assert not env._tack_constraints + assert not env._weld_constraints + + +def test_wet_joint_survives_a_release_impulse(env_and_task): + """A flush joint takes the arm's parting shove as a unit, and still cures. + + Regression: a placement that ended flush against its neighbor could + fling an already-placed span ~5 cm and ~90 degrees during the cure + wait (~30% of flush placements), which is what pushed agents onto a + narrow 3-8 mm assembly gap. The wet-glue tack cannot cancel the + impulse -- momentum is momentum, the assembly still slides -- but + the joint must not come apart while it cures. The newcomer is + joined to an already-welded pair, the mass asymmetry that makes an + untacked joint separate (~11 mm here) rather than slide together. + """ + env, task = env_and_task + span0, span1 = _stage_flush_pair(env, task) + for _ in range(env.cure_threshold + 5): + env.step(_hold_action(env)) + assert env._weld_constraints + + state = env._get_state() + span2 = next(b for b in state.get_objects(env._block_type) + if b.name == "span2") + s = state.copy() + s.set(span2, "x", s.get(span1, "x") + 2 * env.span_half_extents[0]) + s.set(span2, "y", s.get(span1, "y")) + s.set(span2, "z", s.get(span1, "z")) + for feat in ("roll", "pitch", "yaw"): + s.set(span2, feat, 0.0) + env._set_state(s) + env._set_attr(span1, "glue_end_b", 1.0) + env.step(_hold_action(env)) + + before = env._get_state() + rel_before = np.array( + [before.get(span2, f) - before.get(span1, f) for f in ("x", "y", "z")]) + # The shove the arm leaves behind when it releases and retreats. + p.resetBaseVelocity(span2.id, (-2.0, 0.0, 0.0), (0.0, 0.0, 0.0), + physicsClientId=env._physics_client_id) + for _ in range(env.cure_threshold + 5): + env.step(_hold_action(env)) + after = env._get_state() + rel_after = np.array( + [after.get(span2, f) - after.get(span1, f) for f in ("x", "y", "z")]) + assert abs(after.get(span1, "x") - before.get(span1, "x")) > 0.005, \ + "the shove should still move the assembly" + assert np.linalg.norm(rel_after - rel_before) < 0.001 + assert abs(after.get(span2, "yaw") - after.get(span1, "yaw")) < 0.01 + assert after.get(span1, "attached_end_b") == \ + float(env._block_index[span2.name]) From c8ed44c19b971d508dd2d4d9d5c8a3554a18a371 Mon Sep 17 00:00:00 2001 From: Yichao Liang Date: Wed, 19 Aug 2026 06:43:34 -0400 Subject: [PATCH 14/30] bridge: drop the validated-IK override Bridge was the only domain running with pybullet_ik_validate on (common.yaml turns it off everywhere else), on the theory that the cure gates need placement accuracy. Validation is not what enforces that -- a goal-IK branch is accepted on its forward-kinematics error against move_to_pose_tol either way -- and it steered the arm into worse-executing IK branches. Measured on the oracle demonstrator, seeds 0-3, 6 tasks each: 22/24 with validation on, 24/24 with it off (both failures under validation were process-plan exhaustion). --- scripts/configs/predicatorv3/envs/all.yaml | 4 ---- .../test_oracle_process_planning_bridge.py | 12 +++++++----- 2 files changed, 7 insertions(+), 9 deletions(-) diff --git a/scripts/configs/predicatorv3/envs/all.yaml b/scripts/configs/predicatorv3/envs/all.yaml index be853f3d9..e19c085f4 100644 --- a/scripts/configs/predicatorv3/envs/all.yaml +++ b/scripts/configs/predicatorv3/envs/all.yaml @@ -401,10 +401,6 @@ ENVS: # Longest legitimate wait: cure_threshold (25) + cure-start # stagger across the preceding option (~60 steps). wait_option_max_steps: 120 - # Bridge NEEDS validated IK (common.yaml turns it off): placement - # accuracy feeds the cure gates, and lateral placement error is - # frozen into the weld. - pybullet_ik_validate: True # The packed full-variant staging grid leaves ~1-2 cm clearances # that stochastically dip into 2-3 mm grazes; the default 1 mm # margin turns those into unrecoverable BiRRT start/goal diff --git a/tests/approaches/test_oracle_process_planning_bridge.py b/tests/approaches/test_oracle_process_planning_bridge.py index 98ce98e68..7c9b49793 100644 --- a/tests/approaches/test_oracle_process_planning_bridge.py +++ b/tests/approaches/test_oracle_process_planning_bridge.py @@ -60,11 +60,13 @@ def _oracle_bridge_config() -> dict: "wait_option_max_steps": 120, # --- common flags relevant to bilevel refinement --- "skill_phase_use_motion_planning": True, - # Bridge NEEDS validated IK (unlike boil): placement accuracy - # feeds the cure gates, and the lateral placement error is - # frozen into the weld. Unvalidated IK leaves the seated span - # outside the far leg's cure window. - "pybullet_ik_validate": True, + # Bridge follows common.yaml: validated IK OFF, like every + # other domain. Validation is not what enforces placement + # accuracy (goal-IK branches are accepted on forward-kinematics + # error either way) and it steered the arm into + # worse-executing IK branches (oracle sweep: 22/24 with it on, + # 24/24 with it off). + "pybullet_ik_validate": False, "planning_filter_unreachable_nsrt": False, "no_repeated_arguments_in_grounding": True, "terminate_on_goal_reached": False, From 85741db800b781d8cac83f5bd587936fcccef7af Mon Sep 17 00:00:00 2001 From: Yichao Liang Date: Wed, 19 Aug 2026 06:43:35 -0400 Subject: [PATCH 15/30] bridge: drop the -0.02 shallow-held margin override The override papered over plan-start configs that modeled a just-picked assembly 9-21 mm inside the surface it was lifted off. Those poses were real states produced by weld creep (7-9 mm of skate plus 0.13 rad of yaw per 200 idle steps) and the ~15 mm plant-sag place bias, both fixed since (weld anti-creep re-anchoring, verified place release); static supports also get unbounded start-escape allowances now. Measured with instrumented planning calls at the default -0.006: oracle demonstrator seeds 0-3, 24/24 solved, and no held-assembly start contact ever exceeded 3 mm (the artifact the override was sized for was 9-21 mm). --- scripts/configs/predicatorv3/envs/all.yaml | 9 --------- tests/approaches/test_oracle_process_planning_bridge.py | 4 ---- 2 files changed, 13 deletions(-) diff --git a/scripts/configs/predicatorv3/envs/all.yaml b/scripts/configs/predicatorv3/envs/all.yaml index e19c085f4..dfa4ef611 100644 --- a/scripts/configs/predicatorv3/envs/all.yaml +++ b/scripts/configs/predicatorv3/envs/all.yaml @@ -414,12 +414,3 @@ ENVS: # in the codebase; path fidelity is worth the ~2x steps per # motion-planned phase (episodes stay far under the horizon). pybullet_birrt_path_subsample_ratio: 1 - # Right after a grasp/pick, the planning-time model of the held - # object occasionally shows it up to ~14 mm into the table it is - # resting on or was just lifted off (a flaky reconstruction / - # transient artifact -- physically impossible for a lifted - # block), which rejects the next phase's start config. Deepen - # the shallow-held allowance past the artifact (start contacts - # only; the first motion is away from the surface, and planning - # away from a genuine wedge is the right recovery anyway). - pybullet_birrt_shallow_held_contact_margin: -0.02 diff --git a/tests/approaches/test_oracle_process_planning_bridge.py b/tests/approaches/test_oracle_process_planning_bridge.py index 7c9b49793..059aaa1ab 100644 --- a/tests/approaches/test_oracle_process_planning_bridge.py +++ b/tests/approaches/test_oracle_process_planning_bridge.py @@ -49,10 +49,6 @@ def _oracle_bridge_config() -> dict: # default 1 mm margin turns those into unrecoverable BiRRT # start/goal rejections. "pybullet_birrt_contact_margin": -0.005, - # Post-grasp modeling artifacts can show the held object up - # to ~14 mm into its pick surface at the next phase's start - # config; allow escaping those (start contacts only). - "pybullet_birrt_shallow_held_contact_margin": -0.02, # Each Wait ends on the FIRST atom change, so a plan waiting on # several concurrent cures can need a cheap replan for the tail # (which reduces to "Wait until the remaining joint cures"). From a0d2b244814f04e16f7cde6952be304b837e8b50 Mon Sep 17 00:00:00 2001 From: Yichao Liang Date: Wed, 19 Aug 2026 06:50:19 -0400 Subject: [PATCH 16/30] bridge: enable agent_oracle_hybrid_sim and skip agent_po_predicate_invention_al --- scripts/configs/predicatorv3/exp_bridge.yaml | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/scripts/configs/predicatorv3/exp_bridge.yaml b/scripts/configs/predicatorv3/exp_bridge.yaml index 1c77322c3..687f47a99 100644 --- a/scripts/configs/predicatorv3/exp_bridge.yaml +++ b/scripts/configs/predicatorv3/exp_bridge.yaml @@ -13,5 +13,7 @@ ENVS: bridge: SKIP: False APPROACHES: - agent_po_predicate_invention_al: + agent_oracle_hybrid_sim: SKIP: False + agent_po_predicate_invention_al: + SKIP: True From b7b1c4132353dc031ed316d20490028ea4bd6526 Mon Sep 17 00:00:00 2001 From: Yichao Liang Date: Wed, 19 Aug 2026 06:51:03 -0400 Subject: [PATCH 17/30] bridge: remove waypoint subsampling settings for pybullet_birrt --- scripts/configs/predicatorv3/envs/all.yaml | 10 +--------- 1 file changed, 1 insertion(+), 9 deletions(-) diff --git a/scripts/configs/predicatorv3/envs/all.yaml b/scripts/configs/predicatorv3/envs/all.yaml index dfa4ef611..6ba499880 100644 --- a/scripts/configs/predicatorv3/envs/all.yaml +++ b/scripts/configs/predicatorv3/envs/all.yaml @@ -405,12 +405,4 @@ ENVS: # that stochastically dip into 2-3 mm grazes; the default 1 mm # margin turns those into unrecoverable BiRRT start/goal # rejections. - pybullet_birrt_contact_margin: -0.005 - # Execute EVERY planned waypoint (no subsampling): a carried span - # can travel 5-8 cm per physics step between subsampled - # waypoints, and that corner-cutting swept a just-picked span - # through a standing leg's top corner (a 0.3 mm graze topples the - # 2:1 leg). The bridge's standing legs are the tippiest obstacles - # in the codebase; path fidelity is worth the ~2x steps per - # motion-planned phase (episodes stay far under the horizon). - pybullet_birrt_path_subsample_ratio: 1 + pybullet_birrt_contact_margin: -0.005 \ No newline at end of file From 6d554a530d122bd2e1400262ee9c44236a9e0c5f Mon Sep 17 00:00:00 2001 From: Yichao Liang Date: Wed, 19 Aug 2026 07:45:10 -0400 Subject: [PATCH 18/30] env: grasp detection requires a pinch, not a touch (position control) A grasp used to be granted off a SINGLE finger with an aligned closest point within 0.5 mm. When a pick's grasp_z_offset put the pads' grip band at a block's top corners, the closing fingers cammed over the top (shoving the block into the table), one finger's corner graze passed the check, and the constraint froze the block dangling below the gripper - a ghost grasp that doomed the downstream place (seed0 run_20260819_053515: leg0 pressed into the table, landed 16-18 mm off site, episode dead after refinement exhaustion). Under position control, detection now also requires the PARTNER finger to have an aligned closest point within grasp_partner_tol (3 cm): it must at least be closing in on the object's other side. A finger pressing a perpendicular face (the cam-over's top-face press) fails alignment at any distance, while an off-center pre-pinch (jug handle: first pad touches, second still arriving) stays a legitimate capture. Under "reset" control the fingers teleport closed in one step, so closing-instant closest-point normals are overlap artifacts (a legitimately captured jug handle shows the same vertical partner normal as a cam-over) - those envs keep the single-finger rule. Measured from the failed run's staging: offsets 0.0-0.009 stay proper grasps 14/14; the failure's 0.01 becomes an honest miss 7/7. --- predicators/envs/pybullet_env.py | 82 ++++++++++++++++++++++++++------ 1 file changed, 67 insertions(+), 15 deletions(-) diff --git a/predicators/envs/pybullet_env.py b/predicators/envs/pybullet_env.py index 3aeecbfb4..a1400835c 100644 --- a/predicators/envs/pybullet_env.py +++ b/predicators/envs/pybullet_env.py @@ -125,6 +125,13 @@ class PyBulletEnv(BaseEnv): # terminated. grasp_tol: ClassVar[float] = 5e-2 # for large objects grasp_tol_small: ClassVar[float] = 5e-4 # for small objects + # How far the partner finger may still be from the object when the + # first finger's touch triggers grasp detection, as long as it FACES + # the object with an aligned normal (see _detect_held_object). Covers + # objects sitting off-center between the pads (the partner pad is + # still closing), while a finger pressing a perpendicular face (e.g. + # cammed onto a block top) never qualifies at any distance. + grasp_partner_tol: ClassVar[float] = 3e-2 # Strength of welds created by residual Attach commands. PyBullet's # default (500) sags under cantilevered load; matches the bridge # env's feature-driven weld_max_force. @@ -1886,25 +1893,52 @@ def _detect_held_object(self) -> Optional[int]: currently held. Checks contact between each finger and every graspable body (from _get_object_ids_for_held_check()), using contact-normal alignment to reject touches on the outside of the - gripper. If multiple objects qualify, returns the closest. + gripper. A grasp needs a PINCH, not a touch: one finger within + grasp_tol_small of the object with an aligned normal, and the + partner finger at least FACING the object -- an aligned closest + point within grasp_partner_tol (the partner may still be + arriving when the object sits off-center between the pads, e.g. + a jug handle; the first pad's touch is a legitimate capture + there). Accepting a single finger with no partner check used to + grant degenerate grasps when the pads closed above a block and + cammed over its top corners: the partner finger pressed DOWN on + the top face (normal perpendicular to the pinch axis, so it + fails the alignment check at any distance), and the constraint + froze the block dangling below the gripper instead of failing + the pick honestly. + + The pinch requirement only applies under POSITION control, where + the fingers close incrementally and each step's contact geometry + is physical. Under "reset" control the fingers teleport to the + closed position in a single step, so at the detection instant + they overlap the object arbitrarily and closest-point normals + are artifacts of the overlap (a legitimately-captured jug handle + shows the same vertical partner normal as a cam-over) -- there a + single aligned touch keeps granting the grasp, as before. + + If multiple objects qualify, returns the closest. """ expected_finger_normals = self._get_expected_finger_normals() closest_held_obj = None closest_held_obj_dist = float("inf") + require_pinch = CFG.pybullet_control_mode != "reset" + query_dist = max(self.grasp_tol_small, self.grasp_partner_tol) \ + if require_pinch else self.grasp_tol_small for obj_id in self._get_object_ids_for_held_check(): + aligned_finger_dists = [] for finger_id, expected_normal in expected_finger_normals.items(): assert abs(np.linalg.norm(expected_normal) - 1.0) < 1e-5 - # Find points on the object that are within grasp_tol distance - # of the finger. Note that we use getClosestPoints instead of - # getContactPoints because we still want to consider the object - # held even if there is a tiny distance between the fingers and - # the object. + # Find points on the object near the finger. Note that we + # use getClosestPoints instead of getContactPoints because + # we still want to consider the object held even if there + # is a tiny distance between the fingers and the object. closest_points = p.getClosestPoints( bodyA=self._pybullet_robot.robot_id, bodyB=obj_id, - distance=self.grasp_tol_small, + distance=query_dist, linkIndexA=finger_id, physicsClientId=self._physics_client_id) + finger_dist = None for point in closest_points: # If the contact normal is substantially different from # the expected contact normal, this is probably an object @@ -1912,21 +1946,39 @@ def _detect_held_object(self) -> Optional[int]: # A perfect score here is 1.0 (normals are unit vectors). contact_normal = point[7] score = expected_normal.dot(contact_normal) - # logging.debug(f"With obj {obj_id}, score: {score}") assert -1.01 <= score <= 1.01 # Take absolute as object/gripper could be rotated 180 # degrees in the given axis. if np.abs(score) < 0.9: continue - # Handle the case where multiple objects pass this check - # by taking the closest one. This should be rare, but it - # can happen when two objects are stacked and the robot is - # unstacking the top one. contact_distance = point[8] - if contact_distance < closest_held_obj_dist: - closest_held_obj = obj_id - closest_held_obj_dist = contact_distance + if finger_dist is None or contact_distance < finger_dist: + finger_dist = contact_distance + if finger_dist is not None: + aligned_finger_dists.append(finger_dist) + elif require_pinch: + # This finger neither touches nor faces the object with + # an aligned normal: not a pinch. + break + if not aligned_finger_dists: + continue + if require_pinch and \ + len(aligned_finger_dists) < len(expected_finger_normals): + continue + obj_dist = min(aligned_finger_dists) + # The pinch only triggers once a finger actually reaches the + # object; the larger partner tolerance never starts a grasp + # on its own. + if obj_dist > self.grasp_tol_small: + continue + # Handle the case where multiple objects pass this check by + # taking the closest one. This should be rare, but it can + # happen when two objects are stacked and the robot is + # unstacking the top one. + if obj_dist < closest_held_obj_dist: + closest_held_obj = obj_id + closest_held_obj_dist = obj_dist return closest_held_obj def _create_grasp_constraint(self) -> None: From b5276507553d3606884fc06bf67e7e804e5047f7 Mon Sep 17 00:00:00 2001 From: Yichao Liang Date: Wed, 19 Aug 2026 07:45:23 -0400 Subject: [PATCH 19/30] skills: honest verification failures; pick verifies the lift Verified advancement (Phase.verify_fn) always advanced best-effort once the retry budget ran out. That is right for a place's settle stroke (release where it ended beats aborting) but wrong for phases whose failed verification proves the option's outcome is already lost - pressing on just defers the failure to a downstream option with less context to report it. - Phase.verify_failure_msg: when set, exhausting the verification budget raises OptionExecutionFailure with the message instead of advancing (also when a retry rewind itself fails). - PhaseSkill._terminal now enforces the FINAL phase's verification: executors check terminal before calling the policy, so a final phase's verify_fn never ran in the policy's advance path and the option could end unverified. - create_pick_skill(verify_lift=True): at the end of LiftSlightly the target object must have gained at least half of lift_dz, else the option fails with a message telling the agent to fix its grasp_z_offset. This is the honest failure for grasps contact-level detection cannot reject: pads pinching a block's top EDGE show properly aligned normals on both fingers, but the support drags the block out of the constraint during the lift (proper grasps track the gripper to within a few mm; degenerate ones gain at most a third of the lift). --- .../skill_factories/base.py | 35 ++++++++++++- .../skill_factories/move_to.py | 5 ++ .../skill_factories/pick.py | 49 +++++++++++++++++-- 3 files changed, 84 insertions(+), 5 deletions(-) diff --git a/predicators/ground_truth_models/skill_factories/base.py b/predicators/ground_truth_models/skill_factories/base.py index 0a0566a69..9210fa7b6 100644 --- a/predicators/ground_truth_models/skill_factories/base.py +++ b/predicators/ground_truth_models/skill_factories/base.py @@ -391,6 +391,13 @@ class Phase: bool]] = None retry_to_phase: Optional[str] = None max_retries: int = 0 + # When set, exhausting the verification budget raises + # ``OptionExecutionFailure`` with this message instead of advancing + # best-effort. Use for phases whose failed verification proves the + # option's outcome is already lost -- e.g. a pick whose "grasped" + # object did not rise with the gripper: pressing on only defers the + # failure to a downstream option with less context to report it. + verify_failure_msg: Optional[str] = None class PhaseSkill: @@ -520,7 +527,24 @@ def _terminal(self, state: State, memory: Dict, objects: Sequence[Object], if phase_idx < len(self._phases) - 1: return False phase = self._phases[phase_idx] - return self._phase_is_terminal(phase, state, memory, objects, params) + if not self._phase_is_terminal(phase, state, memory, objects, params): + return False + # Verified advancement for the FINAL phase: _policy's advance + # path (where verify_fn normally runs) is never reached for it, + # because executors check terminal before calling the policy. So + # the option-level terminal enforces it: while a retry or an + # honest failure is still pending, the option is not done -- the + # next policy call resolves it (rewinds, or raises + # verify_failure_msg). Only a best-effort phase (no failure + # message, budget spent) terminates unverified. + if phase.verify_fn is None: + return True + if phase.verify_fn(state, objects, params, self._config): + return True + used = memory.get(_PHASE_RETRY_KEY.format(id(phase)), 0) + if used < phase.max_retries or phase.verify_failure_msg is not None: + return False + return True # ------------------------------------------------------------------ # Phase terminal conditions @@ -691,6 +715,10 @@ def _maybe_retry_phase(self, retry_key = _PHASE_RETRY_KEY.format(id(phase)) used = memory.get(retry_key, 0) if used >= phase.max_retries: + if phase.verify_failure_msg is not None: + raise utils.OptionExecutionFailure( + f"[{self._name}/{phase.name}] " + f"{phase.verify_failure_msg}") logging.debug( "[%s/%s] verification failed after %d retries; " "advancing best-effort.", self._name, phase.name, used) @@ -737,6 +765,11 @@ def _maybe_retry_phase(self, # in collision). A retry is opportunistic: degrade to the # unverified advance (release where the stroke ended, the # pre-verification behavior) instead of aborting the option. + if phase.verify_failure_msg is not None: + raise utils.OptionExecutionFailure( + f"[{self._name}/{phase.name}] " + f"{phase.verify_failure_msg} " + f"(retry rewind also failed: {e})") from e logging.debug( "[%s/%s] retry rewind failed (%s); advancing " "best-effort.", self._name, phase.name, e) diff --git a/predicators/ground_truth_models/skill_factories/move_to.py b/predicators/ground_truth_models/skill_factories/move_to.py index 5b0f04c3e..8e2bc9106 100644 --- a/predicators/ground_truth_models/skill_factories/move_to.py +++ b/predicators/ground_truth_models/skill_factories/move_to.py @@ -159,6 +159,7 @@ def make_move_to_phase( bool]] = None, retry_to_phase: Optional[str] = None, max_retries: int = 0, + verify_failure_msg: Optional[str] = None, ) -> Phase: """Create a MOVE_TO_POSE phase for use in a ``PhaseSkill``. @@ -198,6 +199,9 @@ def make_move_to_phase( retry_to_phase: Name of the phase to rewind to on a failed verification. max_retries: Verification retry budget (see ``Phase``). + verify_failure_msg: When set, exhausting the verification budget + raises ``OptionExecutionFailure`` with this message instead + of advancing best-effort (see ``Phase.verify_failure_msg``). Returns: A ``Phase`` that can be included in a ``PhaseSkill``. @@ -265,4 +269,5 @@ def _target_fn( verify_fn=verify_fn, retry_to_phase=retry_to_phase, max_retries=max_retries, + verify_failure_msg=verify_failure_msg, ) diff --git a/predicators/ground_truth_models/skill_factories/pick.py b/predicators/ground_truth_models/skill_factories/pick.py index 4588ff762..b448d646e 100644 --- a/predicators/ground_truth_models/skill_factories/pick.py +++ b/predicators/ground_truth_models/skill_factories/pick.py @@ -69,6 +69,7 @@ def create_pick_skill( anchor_lift: bool = False, grasp_finger_tol: Optional[float] = None, lift_dz: float = 0.01, + verify_lift: bool = False, param_defs: Optional[Sequence[Tuple[str, float, float]]] = None, ) -> ParameterizedOption: """Create a multi-phase pick skill that grasps and lifts an object. @@ -112,6 +113,19 @@ def create_pick_skill( a neighboring object, which then invalidates the NEXT option's BiRRT start config -- unrecoverable by replanning since the arm physically stays put. + verify_lift: If True, the option only succeeds when the target + object actually rose with the gripper: at the end of + LiftSlightly its pose-fn z must have gained at least half of + ``lift_dz``, else the option raises + ``OptionExecutionFailure`` instead of reporting a successful + pick. This is the honest failure for grasps that contact- + level held detection cannot reject: pads that close above a + block cam over its top corners or pinch its top edge, the + grasp constraint freezes the block dangling below the + gripper, and the support drags it out of the constraint + during the lift -- the block is left (near) its support + while the state claims it is held, and the downstream place + jams it into the support instead of failing here. param_defs: Optional override for the continuous parameter definitions (``(description, low, high)`` triples). The default box spans the whole plausible range for any hand, @@ -166,6 +180,10 @@ def _descend_pose( grasp_z = z + grasp_z_offset _shared["grasp_z"] = grasp_z _shared["grasp_xy_yaw"] = (x, y, yaw) + # The object's own (pose-fn) height while it still rests on its + # support: the lift verification measures the object's rise + # against this. + _shared["rest_pose_z"] = z return x, y, grasp_z, yaw def _slight_lift_pose( @@ -181,6 +199,20 @@ def _slight_lift_pose( x, y, _, yaw = get_target_pose_fn(state, objects, _empty, cfg) return x, y, _shared["grasp_z"] + lift_dz, yaw + def _object_rose_with_gripper( + state: State, + objects: Sequence[Object], + params: Array, + cfg: SkillConfig, + ) -> bool: + del params + z_now = get_target_pose_fn(state, objects, _empty, cfg)[2] + # Half of lift_dz separates cleanly: a properly-held object + # tracks the gripper to within a few mm (constraint sag), while + # a degenerate grasp's object is dragged out of the constraint + # by its support and gains at most a third of the lift. + return bool(z_now - _shared["rest_pose_z"] >= 0.5 * lift_dz) + phases = [] phases.extend([ make_move_to_phase("MoveAbove", _above_pose, @@ -200,10 +232,19 @@ def _slight_lift_pose( finger_direction="close", finger_tol=grasp_finger_tol, ), - make_move_to_phase("LiftSlightly", - _slight_lift_pose, - "closed", - allow_shallow_held_object_contacts=True) + make_move_to_phase( + "LiftSlightly", + _slight_lift_pose, + "closed", + allow_shallow_held_object_contacts=True, + verify_fn=_object_rose_with_gripper if verify_lift else None, + verify_failure_msg=( + "grasp verification failed: the object did not rise " + "with the gripper (it was never actually wrapped by the " + "fingers, or its support dragged it out of the grasp " + "during the lift). Retry the pick with a grasp_z_offset " + "that closes the fingers around the object's body, not " + "above it.") if verify_lift else None) ]) return PhaseSkill(name, From 48f9ca4218eac244bdbb927a4baa389d6b55ac7e Mon Sep 17 00:00:00 2001 From: Yichao Liang Date: Wed, 19 Aug 2026 07:45:36 -0400 Subject: [PATCH 20/30] bridge: verify PickBlock lifts; degenerate top-edge grasp regression test Root cause of the seed0 run_20260819_053515 failure: the agent's plan used grasp_z_offset=0.01 on a standing 10 cm leg, which sits on a sub-millimeter knife edge (0.009 grasps properly 7/7, 0.010 cams over the top corners 7/7 from the run's exact staging). The cam-over shoved the leg 17.6 mm into the table, held detection latched a degenerate constraint off a corner graze, the 3 cm lift left the leg still touching the table, and the place jammed it - 16-18 mm off site0, lying, episode dead. The agent's validation rollouts (4/4 with 0.01) happened to land on the good side of the edge; real execution landed 2.5 mm high and cammed. All six solved runs in the experiment used 0.0. PickBlock now sets verify_lift=True: a pick whose block did not rise with the gripper fails with OptionExecutionFailure instead of handing a ghost-held block to the place - the honest signal that taught seeds 1 and 2 to converge on offset 0.0. The regression test pins all three guards from the failed run's geometry: a single-finger touch with nothing closing on the other side is refused, an off-center pre-pinch is still captured, and PickBlock(leg0)[0.01] from reset must either fail honestly or genuinely lift the leg - never end "done" with the leg still on the table. Also clears two redundant local pybullet imports and an unused variable pylint flagged in this file. --- .../ground_truth_models/bridge/options.py | 8 + tests/envs/test_pybullet_bridge.py | 140 +++++++++++++++++- 2 files changed, 145 insertions(+), 3 deletions(-) diff --git a/predicators/ground_truth_models/bridge/options.py b/predicators/ground_truth_models/bridge/options.py index f85234ecf..540724785 100644 --- a/predicators/ground_truth_models/bridge/options.py +++ b/predicators/ground_truth_models/bridge/options.py @@ -134,6 +134,14 @@ def _get_block_grasp_pose( # a neighboring block, poisoning the next option's BiRRT # start config. lift_dz=0.03, + # A grasp_z_offset near the pads' upper engagement edge + # (>= ~1 cm on a standing leg) makes the closing fingers cam + # over the block's top corners: held detection can still + # latch a degenerate constraint, and the table then drags + # the block out of it during the lift. Verifying the lift + # fails such picks honestly instead of handing a + # ghost-held block to the place. + verify_lift=True, ) # -- PickBottle ------------------------------------------------------ diff --git a/tests/envs/test_pybullet_bridge.py b/tests/envs/test_pybullet_bridge.py index d80d3a55e..c7482838f 100644 --- a/tests/envs/test_pybullet_bridge.py +++ b/tests/envs/test_pybullet_bridge.py @@ -231,7 +231,6 @@ def run_option(opt, objs, params): assert abs(state.get(span1, "y") - ty) < 0.006 assert abs(state.get(span1, "yaw")) < 0.03 finally: - import pybullet as p # pylint: disable=import-outside-toplevel p.disconnect(env._physics_client_id) @@ -272,7 +271,6 @@ def test_sim_data_isolated_between_env_instances(env_and_task): assert fresh.get(leg0, "glue_end_b") == 0.0 assert fresh.get(leg0, "cure_end_b") == 0.0 finally: - import pybullet as p # pylint: disable=import-outside-toplevel p.disconnect(other._physics_client_id) @@ -443,7 +441,7 @@ def test_wet_joint_survives_a_release_impulse(env_and_task): untacked joint separate (~11 mm here) rather than slide together. """ env, task = env_and_task - span0, span1 = _stage_flush_pair(env, task) + _, span1 = _stage_flush_pair(env, task) for _ in range(env.cure_threshold + 5): env.step(_hold_action(env)) assert env._weld_constraints @@ -478,3 +476,139 @@ def test_wet_joint_survives_a_release_impulse(env_and_task): assert abs(after.get(span2, "yaw") - after.get(span1, "yaw")) < 0.01 assert after.get(span1, "attached_end_b") == \ float(env._block_index[span2.name]) + + +def test_degenerate_top_edge_grasp_fails_honestly(): + """A pick that never wraps the block must fail, not report success. + + Regression for seed0 run_20260819_053515: PickBlock(leg0)[0.01] on a + standing 10 cm leg put the pads' grip band a hair above the leg top. + The closing fingers cammed over the top corners (shoving the leg + ~18 mm into the table), held detection latched a constraint off a + single finger's corner graze, the 3 cm lift left the leg still on + the table, and the downstream place jammed it -- episode dead. Two + guards cover it: + + 1. ``_detect_held_object`` requires an aligned touch on BOTH + fingers (a single-finger touch is not a pinch). + 2. The pick skill's lift verification (``verify_lift``): the object + must gain at least half of ``lift_dz``, else the option raises + ``OptionExecutionFailure`` -- catching top-EDGE pinches whose + both-finger contact normals look like a real grasp. + """ + utils.reset_config({ + "env": "pybullet_bridge", + "seed": 0, + "num_train_tasks": 1, + "num_test_tasks": 1, + "skill_phase_use_motion_planning": True, + "pybullet_ik_validate": False, + "pybullet_birrt_contact_margin": -0.005, + "pybullet_birrt_path_subsample_ratio": 1, + }) + from predicators.envs.pybullet_bridge import \ + PyBulletBridgeEnv # pylint: disable=import-outside-toplevel + from predicators.ground_truth_models import \ + get_gt_options # pylint: disable=import-outside-toplevel + env = PyBulletBridgeEnv(use_gui=False) + try: + env.reset("test", 0) + state = env._get_state() + options = {o.name: o for o in get_gt_options(env.get_name())} + leg0 = next(b for b in env._blocks if b.name == "leg0") + robot = env._robot + staged_z = state.get(leg0, "z") + + # --- 1) Detector: a single-finger touch is not a grasp. ------- + # Stage the gripper at grip height beside the standing leg so + # that exactly one finger's inner face overlaps the leg (the + # cam-over contact geometry). Strip the joint hint so the pose + # features drive IK. + def _stage_gripper(dy: float, fingers: float) -> None: + s = state.copy() + sim_state = getattr(s, "simulator_state", None) + if isinstance(sim_state, dict): + sim_state = dict(sim_state) + sim_state.pop("joint_positions", None) + s.simulator_state = sim_state + s.set(robot, "x", state.get(leg0, "x")) + s.set(robot, "y", state.get(leg0, "y") + dy) + s.set(robot, "z", staged_z + 0.03) + s.set(robot, "wrist", 0.0) + s.set(robot, "fingers", fingers) + env._set_state(s) + + def _aligned_fingers() -> list: + normals = env._get_expected_finger_normals() + aligned = [] + for fid, normal in normals.items(): + pts = p.getClosestPoints( + bodyA=env._pybullet_robot.robot_id, + bodyB=leg0.id, + distance=env.grasp_tol_small, + linkIndexA=fid, + physicsClientId=env._physics_client_id) + aligned.append( + any(abs(float(normal.dot(pt[7]))) >= 0.9 for pt in pts)) + return aligned + + # One pad overlaps the leg (aligned touch) while the partner pad + # is ~37 mm from any leg surface -- beyond grasp_partner_tol, so + # nothing is closing in on the other side. + _stage_gripper(dy=0.0225, fingers=env.open_fingers) + # The staging is the old detector's grant condition: one finger + # has an aligned touch... + assert sorted(_aligned_fingers()) == [False, True] + # ...and the pinch rule refuses it. + assert env._detect_held_object() is None + + # An off-center pre-pinch stays a legitimate capture: one pad + # touches while the partner pad FACES the leg from ~15 mm + # (within grasp_partner_tol) -- the jug-handle pattern. + _stage_gripper(dy=-0.009, fingers=0.032) + assert env._detect_held_object() == leg0.id + + # Positive control: a genuine straddle (both pads on the leg's + # side faces) still detects. 0.0235 leaves both pads slightly + # inside the 5 cm leg even with IK centering the gripper ~1 mm + # off the commanded xy. + _stage_gripper(dy=0.0, fingers=0.0235) + assert _aligned_fingers() == [True, True] + assert env._detect_held_object() == leg0.id + + # --- 2) Skill: the seed0 pick must fail honestly. ------------- + def run_pick(grasp_z_offset: float): + env.reset("test", 0) + ground = options["PickBlock"].ground([robot, leg0], + np.array([grasp_z_offset], + dtype=np.float32)) + st = env._get_state() + assert ground.initiable(st) + for _ in range(200): + env.step(ground.policy(st)) + st = env._get_state() + if ground.terminal(st): + return st + raise AssertionError("PickBlock did not terminate") + + try: + st = run_pick(0.01) + except utils.OptionExecutionFailure: + # The honest outcome: the pick reports its own failure + # (lift verification, or a collision abort from the + # crushed-in gripper). + pass + else: + # Physics drift may one day land this knife-edge pick as a + # genuine grasp; that is success, not regression. What must + # never happen again is the silent middle: option "done", + # state claims held, leg still (near) the table. + assert st.get(leg0, "is_held") > 0.5 + assert st.get(leg0, "z") > staged_z + 0.015 + + # --- 3) Control: the reliable offset still picks properly. ---- + st = run_pick(0.0) + assert st.get(leg0, "is_held") > 0.5 + assert st.get(leg0, "z") > staged_z + 0.015 + finally: + p.disconnect(env._physics_client_id) From dee20b2744aede93ad8dca7cc343b0870dd3d6df Mon Sep 17 00:00:00 2001 From: Yichao Liang Date: Wed, 19 Aug 2026 10:32:35 -0400 Subject: [PATCH 21/30] types: fix the two remaining mypy errors finger_dist was assigned a bare None, so mypy inferred its type as None and flagged the `contact_distance < finger_dist` branch as unreachable. Annotate it Optional[float], which is what the loop below already assumes. create_pybullet_block takes orientation as a 4-tuple, but the motion planning tests passed a list. Only one of the four call sites was reported, because the other three sit in function bodies mypy does not check; fix all four so they do not surface later. --- predicators/envs/pybullet_env.py | 2 +- tests/pybullet_helpers/test_motion_planning.py | 8 ++++---- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/predicators/envs/pybullet_env.py b/predicators/envs/pybullet_env.py index a1400835c..24d83a624 100644 --- a/predicators/envs/pybullet_env.py +++ b/predicators/envs/pybullet_env.py @@ -1938,7 +1938,7 @@ def _detect_held_object(self) -> Optional[int]: distance=query_dist, linkIndexA=finger_id, physicsClientId=self._physics_client_id) - finger_dist = None + finger_dist: Optional[float] = None for point in closest_points: # If the contact normal is substantially different from # the expected contact normal, this is probably an object diff --git a/tests/pybullet_helpers/test_motion_planning.py b/tests/pybullet_helpers/test_motion_planning.py index 5cd47c3c3..bc05a1b11 100644 --- a/tests/pybullet_helpers/test_motion_planning.py +++ b/tests/pybullet_helpers/test_motion_planning.py @@ -155,7 +155,7 @@ def test_bystander_clearance(physics_client_id): half_extents=(0.2, 0.01, 0.3), mass=0, friction=1, - orientation=[0., 0., 0., 1.], + orientation=(0., 0., 0., 1.), physics_client_id=physics_client_id) p.resetBasePositionAndOrientation(block_id, (1.35, 0.6, 0.5), [0., 0., 0., 1.], @@ -216,7 +216,7 @@ def test_robot_start_escape(physics_client_id): half_extents=(0.03, 0.03, 0.03), mass=0, friction=1, - orientation=[0., 0., 0., 1.], + orientation=(0., 0., 0., 1.), physics_client_id=physics_client_id) def _min_robot_dist(z: float) -> float: @@ -326,7 +326,7 @@ def _plan_around_wall(mass: float, seed: int): half_extents=(0.2, 0.01, 0.3), mass=mass, friction=1, - orientation=[0., 0., 0., 1.], + orientation=(0., 0., 0., 1.), physics_client_id=physics_client_id) # Slide the wall toward the arm until the start config is just # within the bystander clearance of it (earning partner status) @@ -440,7 +440,7 @@ def test_held_attachments(physics_client_id): half_extents=(0.05, 0.05, 0.05), mass=0, friction=1, - orientation=[0., 0., 0., 1.], + orientation=(0., 0., 0., 1.), physics_client_id=physics_client_id) p.resetBasePositionAndOrientation(obstacle_id, np.add(attached_position, From ae7f56c6e5abd053b013c7b339f4f562f74ad392 Mon Sep 17 00:00:00 2001 From: Yichao Liang Date: Thu, 20 Aug 2026 16:19:12 -0400 Subject: [PATCH 22/30] motion planning: held probe runs for every body at both endpoints The endpoint loop skipped the held-proximity probe for any body already in contact_partners - including bodies that had just earned partner status via robot proximity in the same iteration. Two consequences, both against the documented contract: - held_near_endpoint missed such bodies, so held_body_clearances held the held assembly to the wide held-bystander clearance (1 cm in bridge) against an intended contact partner: a place goal with a finger pad within the bystander clearance of the support made the goal config collide and silently degraded the phase to non-collision-checked incremental IK. - endpoint_partners[1] missed a robot-earned start partner the held assembly deliberately approaches at the goal, so a movable placement neighbor was wrongly demoted to the zero margin outside the start neighborhood, over-rejecting flush goal placements. Run the held probe for every body whenever a held assembly exists; the robot probe no longer short-circuits it. --- .../pybullet_helpers/motion_planning.py | 47 +++++++++++++------ 1 file changed, 32 insertions(+), 15 deletions(-) diff --git a/predicators/pybullet_helpers/motion_planning.py b/predicators/pybullet_helpers/motion_planning.py index f3fac91cd..d76908570 100644 --- a/predicators/pybullet_helpers/motion_planning.py +++ b/predicators/pybullet_helpers/motion_planning.py @@ -20,6 +20,15 @@ # path may keep such a contact, but never more than this much deeper # than it began (meters). _START_ESCAPE_DEPTH_SLACK = 0.003 +# Deepest robot-link start contact that still earns the start-escape +# allowance. Deliberately its own bound rather than the shallow +# held-object margin: the rule's motivating reconstruction error is a +# finger or wrist link 5-15 mm inside the object it just grasped or +# settled onto (execution-side sag and settle are not in the feature +# model), well beyond the ~6 mm shallow margin. The allowance is +# escape-only (never deeper than the start, only near the start), so a +# generous depth here cannot be exploited elsewhere on the path. +_ROBOT_START_ESCAPE_MAX_DEPTH = -0.02 # ... and only while within this max-abs joint distance of the start # configuration (radians for revolute joints). Beyond it -- and at any # goal further away -- full margins apply, so the allowance cannot be @@ -108,8 +117,9 @@ def run_motion_planning( ROBOT links get an analogous (always-on) start-escape allowance: a robot-vs-body contact already present at the start configuration - and no deeper than the shallow margin does not reject the path near - the start, as long as it never deepens beyond how it began (plus a + and no deeper than ``_ROBOT_START_ESCAPE_MAX_DEPTH`` does not + reject the path near the start, as long as it never deepens beyond + how it began (plus a small slack) and the configuration stays within a joint-space radius of the start. The planning scene is reconstructed from observable features, so a phase that begins right after a grasp or @@ -206,8 +216,9 @@ def _set_state(pt: JointPositions) -> None: # inside the object it just touched (execution-side sag and settle # are not in the feature model). The start configuration is a fact, # not a choice -- rejecting it fails the whole option with - # certainty -- so a start contact no deeper than the shallow margin - # gets a per-body escape allowance: near the start the path may + # certainty -- so a start contact no deeper than + # _ROBOT_START_ESCAPE_MAX_DEPTH gets a per-body escape allowance: + # near the start the path may # keep that contact, never more than _START_ESCAPE_DEPTH_SLACK # deeper than it began, and only within # _START_LOCAL_JOINT_RADIUS of the start configuration. Deeper @@ -224,7 +235,7 @@ def _set_state(pt: JointPositions) -> None: if not depths: continue start_depth = min(depths) - if start_depth >= shallow_margin: + if start_depth >= _ROBOT_START_ESCAPE_MAX_DEPTH: allowed_robot_escape_margins[body] = \ start_depth - _START_ESCAPE_DEPTH_SLACK @@ -270,16 +281,22 @@ def _set_state(pt: JointPositions) -> None: physicsClientId=physics_client_id): contact_partners.add(body) endpoint_partners[endpoint_idx].add(body) - continue - # Evaluate held proximity at BOTH endpoints, even for a - # body already seen near the other one: partner status - # (within the clearance) at EITHER endpoint must win. - # Skipping bodies already in held_near_endpoint once - # made a butt-joint neighbor 3.1 mm away at the start - # but 1.8 mm away at the (re-aimed) goal a permanent - # bystander, and its own goal proximity then rejected - # the plan. - if not held_assembly or body in contact_partners: + # Evaluate held proximity at EVERY endpoint for EVERY + # body, even one already a partner: partner status + # (within the clearance) at EITHER endpoint must win, + # and this probe also feeds held_near_endpoint and the + # per-endpoint demotion bookkeeping. Skipping bodies + # already seen near the other endpoint once made a + # butt-joint neighbor 3.1 mm away at the start but + # 1.8 mm away at the (re-aimed) goal a permanent + # bystander; skipping bodies the robot is near left a + # robot-earned start partner that the held assembly + # deliberately approaches at the goal out of + # endpoint_partners[1] (wrongly demoting it to the + # zero margin) and out of held_near_endpoint (wrongly + # holding an intended contact to the wide held + # clearance). + if not held_assembly: continue held_dists: List[float] = [] for assembly_body, _ in held_assembly: From 69d6ee897370746eae7b07dd96b4fb0bb7e3fbb5 Mon Sep 17 00:00:00 2001 From: Yichao Liang Date: Thu, 20 Aug 2026 16:19:12 -0400 Subject: [PATCH 23/30] motion planning: robot start escape gets its own depth bound The start-escape rule for robot links gated on the shallow held-object margin. When the bridge -0.02 override was dropped (measured only on held-assembly start contacts), the robot-link escape window silently shrank to the default 6 mm - hard-rejecting the 7-15 mm reconstruction artifacts that motivated the rule. The masking test config override is removed, so test_robot_start_escape now exercises the default margins and pins the window. Give the rule its own _ROBOT_START_ESCAPE_MAX_DEPTH = -0.02: the allowance stays escape-only (never deeper than the start, only near the start), so the generous depth cannot be exploited elsewhere. --- .../pybullet_helpers/test_motion_planning.py | 30 ++++++++++--------- 1 file changed, 16 insertions(+), 14 deletions(-) diff --git a/tests/pybullet_helpers/test_motion_planning.py b/tests/pybullet_helpers/test_motion_planning.py index bc05a1b11..e22e53e90 100644 --- a/tests/pybullet_helpers/test_motion_planning.py +++ b/tests/pybullet_helpers/test_motion_planning.py @@ -192,16 +192,18 @@ def test_robot_start_escape(physics_client_id): """A start config with a shallow robot-vs-body contact still plans. The planning scene is reconstructed from observable features, so a - phase that begins right after a grasp or a settled place can model - a finger or wrist link several mm inside the object it just - touched. Such a start is a fact, not a choice: it must not reject - the whole plan; the path escapes the contact instead (never going - deeper than it began). Start penetration deeper than the shallow - margin still rejects. + phase that begins right after a grasp or a settled place can model a + finger or wrist link several mm inside the object it just touched. + Such a start is a fact, not a choice: it must not reject the whole + plan; the path escapes the contact instead (never going deeper than + it began). Start penetration deeper than the dedicated + ``_ROBOT_START_ESCAPE_MAX_DEPTH`` bound still rejects. Deliberately + run with the default shallow held-object margin: the escape window + must not depend on it (it once did, and dropping a bridge margin + override silently narrowed the window to 6 mm). """ utils.reset_config({ "pybullet_birrt_contact_margin": -0.001, - "pybullet_birrt_shallow_held_contact_margin": -0.02, }) ee_home_position = (1.35, 0.75, 0.75) ee_orn = p.getQuaternionFromEuler([0.0, np.pi / 2, -np.pi]) @@ -295,13 +297,13 @@ def _min_robot_dist(z: float) -> float: def test_start_local_partner_demotion(physics_client_id): """Partner status earned only at the start expires with the start. - A movable body the robot merely begins near is checked with the - hard contact margin only inside the start neighborhood; beyond it - the path may touch the body but not penetrate it. Otherwise a body - grazed on the way out of the start keeps a penetration allowance - for the entire path, which physically shoves it (a bottle retreat - after a glue dab repeatedly nudged an assembled row this way). - Static bodies cannot be shoved and keep their partner margin. + A movable body the robot merely begins near is checked with the hard + contact margin only inside the start neighborhood; beyond it the + path may touch the body but not penetrate it. Otherwise a body + grazed on the way out of the start keeps a penetration allowance for + the entire path, which physically shoves it (a bottle retreat after + a glue dab repeatedly nudged an assembled row this way). Static + bodies cannot be shoved and keep their partner margin. """ utils.reset_config({ "pybullet_birrt_contact_margin": -0.03, From e4b5825bc37a4e6476aceb6282f4ee16c514f54d Mon Sep 17 00:00:00 2001 From: Yichao Liang Date: Thu, 20 Aug 2026 16:19:13 -0400 Subject: [PATCH 24/30] skills: goal-IK candidates pin finger joints to the current config Restart seeds randomize every joint and IK leaves the fingers wherever the seed put them (they do not move the EE pose), so restart-derived goal candidates carried essentially random finger values. The chosen candidate's goal config is collision-checked (and the goal-side BiRRT tree grown) at those values while replay drives the fingers per finger_status - a fallback branch checked with near-closed fingers could execute with open fingers, under-checking gripper clearance in exactly the cluttered scenes where fallback branches get picked. It also made dedup treat arm-identical branches as distinct. Pin accepted candidates' finger entries to the current finger positions (after the FK accuracy check, which fingers cannot affect). The fake goal-IK robot grows finger indices and the exact-value expectations assert the pinning. Also fixes a stale _solve_goal_ik comment reference. --- .../skill_factories/base.py | 16 +++++++++++++- tests/test_skill_factories.py | 21 +++++++++++++------ 2 files changed, 30 insertions(+), 7 deletions(-) diff --git a/predicators/ground_truth_models/skill_factories/base.py b/predicators/ground_truth_models/skill_factories/base.py index 9210fa7b6..1bda0a4bc 100644 --- a/predicators/ground_truth_models/skill_factories/base.py +++ b/predicators/ground_truth_models/skill_factories/base.py @@ -923,7 +923,7 @@ def _execute_gentle_stroke(self, phase: Phase, state: State, memory: Dict, # horizon (where the thrashing arm bulldozes the scene). _ik_stall_window: ClassVar[int] = 25 # Random in-limit IK restarts for the BiRRT goal solve, tried after - # the current-joints and home seeds (see _solve_goal_ik). + # the current-joints and home seeds (see _solve_goal_ik_candidates). _goal_ik_num_restarts: ClassVar[int] = 8 # Escalated restart count for the goal solve, used when every branch # the normal solve found puts the goal configuration in collision @@ -1694,6 +1694,20 @@ def _solve_goal_ik_candidates( np.square( np.subtract(ee_position, target_pose.position)))) if err < self._config.move_to_pose_tol: + # IK leaves the finger joints wherever the seed put + # them (they do not move the EE pose, so the + # accuracy check above is indifferent), and the + # restart seeds randomize every joint - so restart- + # derived candidates would carry arbitrary finger + # values. The chosen candidate's goal is collision- + # checked (and the goal-side BiRRT tree grown) at + # those values while replay drives the fingers per + # finger_status, so pin them to the current finger + # positions; this also keeps the dedup below from + # treating arm-identical branches as distinct. + for f_idx in (planning_robot.left_finger_joint_idx, + planning_robot.right_finger_joint_idx): + clamped[f_idx] = float(current_joints[f_idx]) if not any( max(abs(a - b) for a, b in zip(clamped, prior)) < 1e-3 diff --git a/tests/test_skill_factories.py b/tests/test_skill_factories.py index db454b068..eaf9e2ce9 100644 --- a/tests/test_skill_factories.py +++ b/tests/test_skill_factories.py @@ -1335,6 +1335,10 @@ class _FakeGoalIkRobot: joint_lower_limits = [-3.0] * 7 joint_upper_limits = [3.0] * 7 initial_joint_positions = [0.0] * 7 + # Accepted candidates get their finger entries pinned to the current + # finger positions (IK leaves fingers wherever the seed put them). + left_finger_joint_idx = 5 + right_finger_joint_idx = 6 def __init__(self, target_pose: Pose, one_shot_error_m: float) -> None: self._target = target_pose @@ -1399,8 +1403,9 @@ def test_inaccurate_one_shot_escalates_to_validated(self, robot_scene): target, [0.5] * 7, validate=False) # The validated branch is the only accurate one; every seed - # escalates to it and dedup collapses them to one candidate. - assert result == [[0.1] * 7] + # escalates to it and dedup collapses them to one candidate + # (fingers pinned to the current joints). + assert result == [[0.1] * 5 + [0.5] * 2] assert fake.validated_calls == _GOAL_IK_NUM_SEEDS def test_accurate_one_shot_keeps_fast_path(self, robot_scene): @@ -1413,7 +1418,7 @@ def test_accurate_one_shot_keeps_fast_path(self, robot_scene): fake, target, [0.5] * 7, validate=False) - assert result == [[0.2] * 7] + assert result == [[0.2] * 5 + [0.5] * 2] assert fake.validated_calls == 0 def test_distinct_branches_are_all_collected(self, robot_scene): @@ -1434,7 +1439,11 @@ def test_distinct_branches_are_all_collected(self, robot_scene): fake, target, [0.5] * 7, validate=False) - assert result == [[0.0] * 7, [0.1] * 7, [0.2] * 7] + assert result == [ + [0.0] * 5 + [0.5] * 2, + [0.1] * 5 + [0.5] * 2, + [0.2] * 5 + [0.5] * 2, + ] def test_all_branches_inaccurate_raises(self, robot_scene): """When no branch hits the pose, goal IK raises instead of handing @@ -1503,8 +1512,8 @@ def _make_fake(): class TestCollisionDiagnosticsLogging: - """The diagnostics can be computed quietly, before the failure is - known to be final.""" + """The diagnostics can be computed quietly, before the failure is known to + be final.""" def _make_skill(self, robot) -> PhaseSkill: config = _make_config(robot) From d01252bb1b02a6f7351455b48121596c73da5289 Mon Sep 17 00:00:00 2001 From: Yichao Liang Date: Thu, 20 Aug 2026 16:19:13 -0400 Subject: [PATCH 25/30] style: docformatter conformance; restore all.yaml trailing newline docformatter reflows in pybullet_bridge.py and the two test files (pure reflow, no wording changes); scripts/configs/predicatorv3/envs/ all.yaml lost its trailing newline in the margin-override drop. --- predicators/envs/pybullet_bridge.py | 3 ++- scripts/configs/predicatorv3/envs/all.yaml | 2 +- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/predicators/envs/pybullet_bridge.py b/predicators/envs/pybullet_bridge.py index ca7d65dbd..32d52edde 100644 --- a/predicators/envs/pybullet_bridge.py +++ b/predicators/envs/pybullet_bridge.py @@ -1456,7 +1456,8 @@ def _latch_joint(self, state: State, blk: Object, face: str, """Irreversibly attach ``blk.face`` to ``mate``: record the partnership on both blocks, consume the glue, create the weld. - Returns whether the joint latched.""" + Returns whether the joint latched. + """ mate_slot = self._mate_slot_for(state, blk, face, mate) if self._attr(mate, f"attached_{mate_slot}", -1.0) >= 0: # The mate's slot is somehow taken; refuse to latch rather diff --git a/scripts/configs/predicatorv3/envs/all.yaml b/scripts/configs/predicatorv3/envs/all.yaml index 6ba499880..3ce9b85d4 100644 --- a/scripts/configs/predicatorv3/envs/all.yaml +++ b/scripts/configs/predicatorv3/envs/all.yaml @@ -405,4 +405,4 @@ ENVS: # that stochastically dip into 2-3 mm grazes; the default 1 mm # margin turns those into unrecoverable BiRRT start/goal # rejections. - pybullet_birrt_contact_margin: -0.005 \ No newline at end of file + pybullet_birrt_contact_margin: -0.005 From f717a58b3336a339930688f9c0e3a251933d38cf Mon Sep 17 00:00:00 2001 From: Yichao Liang Date: Thu, 20 Aug 2026 17:03:32 -0400 Subject: [PATCH 26/30] test: the hardened bridge oracle e2e passes strictly again The GT-sim hardening in this PR (cure gates, samplers, IK branches, grasp verification) makes the end-to-end solve deterministic; drop the xfail marker the previous PR added at its boundary. --- tests/approaches/test_oracle_process_planning_bridge.py | 7 ------- 1 file changed, 7 deletions(-) diff --git a/tests/approaches/test_oracle_process_planning_bridge.py b/tests/approaches/test_oracle_process_planning_bridge.py index 059aaa1ab..13c8133cd 100644 --- a/tests/approaches/test_oracle_process_planning_bridge.py +++ b/tests/approaches/test_oracle_process_planning_bridge.py @@ -19,8 +19,6 @@ import logging -import pytest - import predicators.approaches # noqa: F401 # pylint: disable=unused-import import predicators.envs # noqa: F401 # pylint: disable=unused-import import predicators.ground_truth_models # noqa: F401 # pylint: disable=unused-import @@ -83,11 +81,6 @@ def _oracle_bridge_config() -> dict: } -@pytest.mark.xfail( - reason="The pre-hardening GT-sim pipeline is not reliable enough for CI\n" - "yet; the hardening changes stacked on this commit make the solve\n" - "deterministic and drop this marker.", - strict=False) def test_oracle_process_planning_solves_bridge_task(): """Smoke test: oracle_process_planning builds the simple n-bridge.""" utils.reset_config(_oracle_bridge_config()) From 9a1a58c4e5a6d8a66905b70e11deea6a361e9a92 Mon Sep 17 00:00:00 2001 From: Yichao Liang Date: Wed, 19 Aug 2026 09:27:18 -0400 Subject: [PATCH 27/30] agents: raise the plan-validation gate to 5 rollouts (10 after flaky) An n-rollout gate passes a plan whose per-rollout success rate is p with probability p^n, so the old 3-rollout gate let marginal plans through: at p=0.85 it passes 61%. Bridge run_20260819_053515 is the motivating case - a grasp offset sitting on a sub-millimeter knife edge validated 3/3 (plus one probe rollout), then cammed out on the single real episode. Five rollouts cut the slip rate to 44% at the same p and much lower for worse plans; the escalated after-flaky gate doubles to 10 since a flaky rejection is direct evidence the agent is tuning in a marginal region. Cost is ~2 extra sim rollouts (seconds) per capture attempt. --- predicators/settings.py | 20 ++++++++++++-------- 1 file changed, 12 insertions(+), 8 deletions(-) diff --git a/predicators/settings.py b/predicators/settings.py index d03df022c..cf22456f0 100644 --- a/predicators/settings.py +++ b/predicators/settings.py @@ -1652,16 +1652,20 @@ class GlobalSettings: # variability the real rollout will - a flaky plan is reported to the # agent in-session (where it can add margin and resubmit) instead of # captured and discovered as a failed real episode. 1 disables repeats. - agent_plan_validation_rollouts = 3 - # Escalated rollout count once a task has produced a FLAKY rejection. - # A 3-rollout gate passes a plan with per-rollout success rate p with - # probability p^3 (p=0.85 -> 61%), so marginal plans slip through and - # die on the single real episode (run_20260717_182321: a 20/20-swept - # relay placement validated 3/3, then missed the target for real). A - # FLAKY rejection is direct evidence the agent is tuning in a marginal + # An n-rollout gate passes a plan with per-rollout success rate p with + # probability p^n, so small n lets marginal plans through: at p=0.85, + # 3 rollouts pass 61% and 5 pass 44% (bridge run_20260819_053515: a + # knife-edge grasp offset validated 3/3, then cammed out on the real + # episode). The agent can request a stricter gate per submission via + # the tool's validation_rollouts argument; it can never lower this. + agent_plan_validation_rollouts = 5 + # Escalated rollout count once a task has produced a FLAKY rejection + # (see the p^n math above; run_20260717_182321: a 20/20-swept relay + # placement validated 3/3, then missed the target for real). A FLAKY + # rejection is direct evidence the agent is tuning in a marginal # region, so subsequent captures on that task must clear this stricter # gate instead. Never lowers the base count. - agent_plan_validation_rollouts_after_flaky = 6 + agent_plan_validation_rollouts_after_flaky = 10 # Run each validation rollout inside ``ctx.validation_env_scope`` (a # freshly constructed sim env) when the approach installs one. A shared # env's reset provably cannot reconstruct state exactly (solver From 7ca1c6b1e67e3f87628731fea3fd16037ad0cc2c Mon Sep 17 00:00:00 2001 From: Yichao Liang Date: Wed, 19 Aug 2026 09:27:32 -0400 Subject: [PATCH 28/30] agents: reproducible, agent-steerable plan validation Validation rollouts sampled execution variability the agent could neither see nor replay: repeats ran at undisclosed decorrelated planner seeds, so a FLAKY report named a failing step but gave no way to re-run that exact draw - the agent could only re-sample and hope (bridge run_20260819_053515: the knife-edge failure mode was only reachable by luck). Seeds are now first-class on both surfaces: - Every validation rollout and probe trial reports the planner seed it ran at; the FLAKY rejection names the failing seed and how to reproduce it. - evaluate_option_plan gains rollout_seed=S: one rollout at exactly that seed on a fresh env with full per-step reporting. Diagnostic only - never captured (an agent-chosen seed must not pass the capture gate) and stale-env scene renders are skipped. - evaluate_option_plan gains validation_rollouts=N: a stricter gate on demand, effective count max(configured, N) - it can raise the gate, never lower it. Combined with rollout_seed it instead runs exactly N diagnostic trials at seeds S..S+N-1, mirroring sim.run. - sim.run (explore_python) gains seed=S: single runs execute at S, trials=N runs trial i at S+i with per-trial seeds in the report, physics sweeps run every point at S. - absolute_rollout_seed joins decorrelated_rollout_seed in context.py as the shared scope both surfaces use. The SUBMIT guidance teaches the loop: flaky -> reproduce the reported seed -> fix the actual failure -> add margin -> resubmit (optionally with a stricter gate). --- predicators/agent_sdk/belief_probe.py | 78 +++++--- predicators/agent_sdk/sketch_prompts.py | 13 +- predicators/agent_sdk/tools/context.py | 25 +++ predicators/agent_sdk/tools/testing.py | 180 ++++++++++++++++-- .../test_evaluate_option_plan_capture.py | 116 +++++++++-- 5 files changed, 355 insertions(+), 57 deletions(-) diff --git a/predicators/agent_sdk/belief_probe.py b/predicators/agent_sdk/belief_probe.py index 5a22ed4c2..b32027311 100644 --- a/predicators/agent_sdk/belief_probe.py +++ b/predicators/agent_sdk/belief_probe.py @@ -37,7 +37,8 @@ from predicators import utils from predicators.agent_sdk.config import RefinementConfig, ToolSurfaceConfig, \ ValidationConfig -from predicators.agent_sdk.tools.context import decorrelated_rollout_seed +from predicators.agent_sdk.tools.context import absolute_rollout_seed, \ + decorrelated_rollout_seed from predicators.agent_sdk.tools.scene import apply_state_modifications, \ draw_pybullet_annotation, render_pybullet_image, render_scene_image from predicators.agent_sdk.tools.verdicts import _EvalStateCollector, \ @@ -247,10 +248,12 @@ class ProbeTrialsResult(_StrLikeResult): ``trials`` holds one dict per trial (``goal_reached``, ``num_actions``, ``failure`` - ``None`` or ``"step {i} ({option}): - {reason}"``; with ``solved=True`` also ``solved``/``reward`` from - the task evaluator, ``None`` when the verdict errored). ``successes`` - counts goal-reaching trials. The current state is NOT advanced - - repeated trials are a measurement, not a navigation step. + {reason}"``; ``planner_seed`` - the motion-planner seed the trial + ran at, reproducible via ``run(plan, seed=...)``; with + ``solved=True`` also ``solved``/``reward`` from the task evaluator, + ``None`` when the verdict errored). ``successes`` counts + goal-reaching trials. The current state is NOT advanced - repeated + trials are a measurement, not a navigation step. """ trials: List[Dict[str, Any]] successes: int @@ -272,13 +275,15 @@ def __repr__(self) -> str: f"evaluator") lines = [f"{headline} ({env_note})"] for i, t in enumerate(self.trials): + seed_tag = (f" (planner seed {t['planner_seed']})" + if t.get("planner_seed") is not None else "") if t["failure"]: - line = f" trial {i + 1}: FAILED - {t['failure']}" + line = f" trial {i + 1}{seed_tag}: FAILED - {t['failure']}" elif t["goal_reached"]: - line = (f" trial {i + 1}: goal reached " + line = (f" trial {i + 1}{seed_tag}: goal reached " f"({t['num_actions']} actions)") else: - line = (f" trial {i + 1}: goal NOT reached " + line = (f" trial {i + 1}{seed_tag}: goal NOT reached " f"({t['num_actions']} actions)") if t.get("solved") is not None: line += (f" - evaluator: solved={t['solved']}, " @@ -852,7 +857,8 @@ def run( trials: int = 1, solved: bool = False, contacts: bool = False, - physics_sweep: bool = False + physics_sweep: bool = False, + seed: Optional[int] = None, ) -> Union[ProbeResult, ProbeTrialsResult, ProbeSweepResult]: """Execute an option plan from the current state. @@ -918,6 +924,15 @@ def run( Rollouts are deterministic per point, so each point costs one rollout and its outcome is a measurement, not a sample. The current state is NOT advanced and nothing is rendered. + + ``seed=S`` overrides the base motion-planner seed for this call. + Trials report the planner seed each ran at (trial ``i`` runs at + ``S + i``; without ``seed=`` at ``base + i``), and + ``evaluate_option_plan``'s validation rollouts report theirs the + same way - so a failed rollout at a reported seed can be + reproduced exactly here: ``run(plan, seed=)``. + A single run (``trials=1``) executes entirely at ``S``; a + physics sweep runs every point at ``S`` instead of the base. """ # pylint: disable-next=import-outside-toplevel import numpy as np @@ -1007,7 +1022,8 @@ def _horizon_note(total_actions: int) -> Optional[str]: # outcome flip between points is attributable to the # physics perturbation alone. with (fresh_scope() if point is None else fresh_scope( - physical_overrides=point)): + physical_overrides=point)), \ + absolute_rollout_seed(seed): model = self._option_model() r = bilevel_sketch.execute_plan_forward( probe_task, @@ -1069,6 +1085,7 @@ def _horizon_note(total_actions: int) -> Optional[str]: # pylint: disable-next=import-outside-toplevel import contextlib trial_dicts: List[Dict[str, Any]] = [] + base_planner_seed = seed if seed is not None else CFG.seed try: for trial_idx in range(trials): _check_time_budget(ctx) @@ -1083,6 +1100,7 @@ def _horizon_note(total_actions: int) -> Optional[str]: # env construction keeps the base seed. with (fresh_scope() if fresh_scope is not None else contextlib.nullcontext()), \ + absolute_rollout_seed(seed), \ decorrelated_rollout_seed(trial_idx): model = self._option_model() collector = (_EvalStateCollector( @@ -1130,12 +1148,20 @@ def _horizon_note(total_actions: int) -> Optional[str]: f"{fs.failure_reason or 'not initiable'}") total = sum(s.num_actions for s in r.steps) trial_dicts.append({ - "goal_reached": r.goal_reached, - "num_actions": total, - "failure": failure, - "solved": trial_solved, - "reward": trial_reward, - "verdict_coarse": coarse, + "goal_reached": + r.goal_reached, + "num_actions": + total, + "failure": + failure, + "solved": + trial_solved, + "reward": + trial_reward, + "verdict_coarse": + coarse, + "planner_seed": + base_planner_seed + trial_idx, }) except ProbeBudgetExceeded as e: # Completed trials are minutes of sim time and live in the @@ -1224,15 +1250,19 @@ def _on_step(i: int, outcome: Any) -> None: contact_env = env contact_env.start_contact_recording() contact_events: List[Dict[str, Any]] = [] + if seed is not None: + notices.append(f"rollout ran at planner seed {seed} (base seed " + f"overridden for this call)") try: - result = bilevel_sketch.execute_plan_forward( - probe_task, - grounded, - model, - predicates=all_predicates, - sketch=sketch_steps, - on_step=_on_step, - stop_on_failure=True) + with absolute_rollout_seed(seed): + result = bilevel_sketch.execute_plan_forward( + probe_task, + grounded, + model, + predicates=all_predicates, + sketch=sketch_steps, + on_step=_on_step, + stop_on_failure=True) finally: if contact_env is not None: contact_events = contact_env.stop_contact_recording() diff --git a/predicators/agent_sdk/sketch_prompts.py b/predicators/agent_sdk/sketch_prompts.py index d02739659..b10345759 100644 --- a/predicators/agent_sdk/sketch_prompts.py +++ b/predicators/agent_sdk/sketch_prompts.py @@ -372,9 +372,16 @@ def _has_tool(name: str) -> bool: "task_idx). When it reaches the goal, that plan is captured as " "your answer, so do NOT finish until evaluate_option_plan " "CONFIRMS the capture. A goal-reaching plan is re-run several " - "times before capture (simulation varies across runs); if it is " - "reported FLAKY, add margin to the fragile step and resubmit. " + - margin_guidance + + "times before capture (simulation varies across runs; each " + "rollout reports the motion-planner seed it ran at); if it is " + "reported FLAKY, reproduce the failed rollout exactly (pass " + "its reported seed as rollout_seed to evaluate_option_plan, or " + "`sim.run(plan_text, seed=...)` in explore_python) to see WHY, " + "then add margin to the fragile step and resubmit. For a plan " + "you suspect is marginal, request a stricter gate up front " + "with validation_rollouts=N (more repeats; never fewer than " + "configured) or measure reliability first with " + "`sim.run(plan_text, trials=N)`. " + margin_guidance + "CAPTURE FIRST, OPTIMIZE SECOND: when the reward charges for " "resources used (read the scoring section), a captured " "modest-reward solve outscores an uncaptured optimal attempt " diff --git a/predicators/agent_sdk/tools/context.py b/predicators/agent_sdk/tools/context.py index 0ffb3d507..1f2681d59 100644 --- a/predicators/agent_sdk/tools/context.py +++ b/predicators/agent_sdk/tools/context.py @@ -304,3 +304,28 @@ def decorrelated_rollout_seed(rollout_idx: int) -> Iterator[None]: yield finally: CFG.seed = base_seed + + +@contextmanager +def absolute_rollout_seed(seed: Optional[int]) -> Iterator[None]: + """Run a scope at an explicit motion-planner seed (None = no-op). + + The agent-facing counterpart of ``decorrelated_rollout_seed``: + validation repeats and probe trials REPORT the planner seed each + rollout ran at, and this scope lets a follow-up call re-run a plan + at exactly that seed - the only way to reproduce a seed-dependent + failure (e.g. one FLAKY validation rollout out of five) instead of + re-sampling and hoping to draw it again. Composes with + ``decorrelated_rollout_seed``: enter this first, and trial ``i`` + runs at ``seed + i``. Enter AFTER any fresh env is created so env + construction (and its task-cache key) still sees the base seed. + """ + if seed is None: + yield + return + base_seed = CFG.seed + CFG.seed = seed + try: + yield + finally: + CFG.seed = base_seed diff --git a/predicators/agent_sdk/tools/testing.py b/predicators/agent_sdk/tools/testing.py index 106a348cf..ae1a8c035 100644 --- a/predicators/agent_sdk/tools/testing.py +++ b/predicators/agent_sdk/tools/testing.py @@ -13,7 +13,7 @@ from predicators.agent_sdk.tools.capture import BestEffortReason, \ CaptureDecision, _decide_capture from predicators.agent_sdk.tools.context import ToolContext, \ - _capture_task_key, decorrelated_rollout_seed + _capture_task_key, absolute_rollout_seed, decorrelated_rollout_seed from predicators.agent_sdk.tools.results import _error_result from predicators.agent_sdk.tools.scene import format_object_poses, \ render_scene_image @@ -23,6 +23,11 @@ load_ground_sampler_fns from predicators.settings import CFG +# Ceiling on agent-requested validation rollouts per submission +# (validation_rollouts): the agent pays for rollouts from its budget, but +# a typo'd request should not silently torch it. +_MAX_REQUESTED_ROLLOUTS = 25 + def _build_testing_tools(ctx: ToolContext, _text_result: Callable, tool: Callable) -> Dict[str, Any]: @@ -137,8 +142,16 @@ async def evaluate_predicate_on_trajectory( "CURRENT task (omit task_idx), it is captured as your answer, and the " "per-step subgoals make it execute closed-loop (monitored, with " "replan-on-divergence). Capture is gated: a goal-reaching plan is " - "re-run several times (simulation varies across runs) and a FLAKY " - "plan is reported instead of captured - add margin and resubmit. " + "re-run several times (simulation varies across runs; each rollout " + "reports the motion-planner seed it ran at) and a FLAKY plan is " + "reported instead of captured - add margin and resubmit. " + "`validation_rollouts` requests a STRICTER gate for this " + "submission (more rollouts; never fewer than configured). " + "`rollout_seed` re-runs the plan at that exact planner seed " + "with full per-step reporting - use it to reproduce and debug a " + "reported failed rollout; combine with validation_rollouts=N for " + "N seeded trials at consecutive seeds. A seeded run is diagnostic " + "only and is never captured. " "When identified physical parameters are active, it is also re-run " "at a grid of perturbations spanning +-1 sigma of those parameters " "(the physics fit's own uncertainty); a PARAM-SENSITIVE plan is " @@ -183,6 +196,30 @@ async def evaluate_predicate_on_trajectory( "Train task index to test on. Omit to use " "the current solve-time task." }, + "validation_rollouts": { + "type": + "integer", + "description": + "Request a stricter capture gate: total validation " + "rollouts a goal-reaching submission must pass. The " + "effective count is max(configured gate, this) - it can " + "raise the gate but never lower it. Use before " + "committing a plan you suspect is marginal. Combined " + "with rollout_seed=S it instead runs exactly this many " + "DIAGNOSTIC trials at planner seeds S, S+1, ... (each " + "outcome reported with its seed; never captured).", + }, + "rollout_seed": { + "type": + "integer", + "description": + "Diagnostic: run the plan at this exact motion-planner " + "seed (as reported per rollout in validation output) " + "with full per-step reporting, to reproduce a failed " + "validation rollout. Add validation_rollouts=N to run " + "N trials at seeds S, S+1, ..., like sim.run(plan, " + "trials=N, seed=S). A seeded run is never captured.", + }, }, "required": ["plan"], }, @@ -212,6 +249,15 @@ async def evaluate_option_plan(args: Dict[str, Any]) -> Dict[str, Any]: plan_text = (args.get("plan") or "").strip() include_states = args.get("include_states", False) include_atoms = args.get("include_atoms", True) + requested_rollouts = args.get("validation_rollouts") + diagnostic_seed = args.get("rollout_seed") + if requested_rollouts is not None and (not isinstance( + requested_rollouts, int) or requested_rollouts < 1): + return _error_result( + "validation_rollouts must be a positive integer.") + if diagnostic_seed is not None and not isinstance( + diagnostic_seed, int): + return _error_result("rollout_seed must be an integer.") resolved, task_err = _resolve_task(ctx, task_idx) if task_err is not None: @@ -312,6 +358,11 @@ def _report_step(i: int, outcome: Any) -> None: step_line += ("\n State:\n" + post.dict_str(indent=4, num_decimal_points=4)) lines.append(step_line) + # A seeded diagnostic rollout runs on a FRESH env (when the + # session provides one), but the renderer draws the shared + # session env - its stale scene would be misleading. + if diagnostic_seed is not None and diag_fresh_scope is not None: + return img_block = render_scene_image(ctx, f"step_{i}_{opt.name}") if img_block and img_block.get("saved_path"): saved_image_paths.append(img_block["saved_path"]) @@ -322,13 +373,36 @@ def _report_step(i: int, outcome: Any) -> None: # continue past a collision and report a goal that the real rollout — # which ends the episode at that failed option — never reaches. ctx.attempt_rollout_count += 1 - result = bilevel_sketch.execute_plan_forward(task, - grounded_plan, - ctx.option_model, - predicates=all_predicates, - sketch=sketch_steps, - on_step=_report_step, - stop_on_failure=True) + # A seeded diagnostic rollout reproduces a validation repeat + # faithfully: fresh env (when the session provides one) plus the + # requested planner seed. diag_fresh_scope is also read by + # _report_step to skip stale-env renders. + diag_fresh_scope = (ctx.validation_env_scope + if diagnostic_seed is not None + and validation_cfg.fresh_env else None) + if diagnostic_seed is not None: + trials_note = ( + f"; running {min(requested_rollouts, _MAX_REQUESTED_ROLLOUTS)}" + " diagnostic trials at consecutive seeds" + if requested_rollouts is not None and requested_rollouts > 1 + else "; validation repeats skipped") + lines.append( + f"DIAGNOSTIC rollout at planner seed {diagnostic_seed}" + + (" on a fresh simulator env" + if diag_fresh_scope is not None else "") + + f" - never captured{trials_note}. Resubmit without " + "rollout_seed to capture.") + with (diag_fresh_scope() if diag_fresh_scope is not None else + contextlib.nullcontext()), \ + absolute_rollout_seed(diagnostic_seed): + result = bilevel_sketch.execute_plan_forward( + task, + grounded_plan, + ctx.option_model, + predicates=all_predicates, + sketch=sketch_steps, + on_step=_report_step, + stop_on_failure=True) final_atoms = utils.abstract(result.final_state, ctx.predicates) # Use the env's goal-check (its own classifiers); robust to invented @@ -444,6 +518,22 @@ def _validation_rollout() -> Tuple[bool, str]: capture_task_key = _capture_task_key(ctx) if capture_task_key in ctx.flaky_capture_task_keys: n_rollouts = max(n_rollouts, validation_cfg.rollouts_after_flaky) + # The agent may request a STRICTER gate for this submission (a + # plan it suspects is marginal); it can never lower the + # configured gate - that would let a lucky draw bypass it. + capped_request: Optional[int] = None + if requested_rollouts is not None: + capped_request = min(requested_rollouts, _MAX_REQUESTED_ROLLOUTS) + if capped_request < requested_rollouts: + lines.append( + f"NOTE: validation_rollouts={requested_rollouts} capped " + f"at {_MAX_REQUESTED_ROLLOUTS}.") + # With rollout_seed the request means "this many diagnostic + # trials", exactly as asked - there is no capture gate to + # protect, so neither the configured gate nor the flaky + # escalation inflates it. + if diagnostic_seed is None: + n_rollouts = max(n_rollouts, capped_request) # Fresh env per validation rollout when the approach provides one: # repeats on the shared env are correlated (its reset cannot # reconstruct state exactly), so only fresh envs sample the same @@ -451,9 +541,10 @@ def _validation_rollout() -> Tuple[bool, str]: fresh_scope = (ctx.validation_env_scope if validation_cfg.fresh_env else None) rollout_outcomes: List[str] = [] + base_planner_seed = CFG.seed if (ctx.capture_goal_reaching_plans and is_current and goal_achieved and not evaluator_rejected and grounded_plan - and n_rollouts > 1): + and diagnostic_seed is None and n_rollouts > 1): # Run ALL validation rollouts even after a failure: the # per-rollout outcome list distinguishes failure modes (a # physics-tail fizzle vs. an IK stall vs. a certificate @@ -472,24 +563,69 @@ def _validation_rollout() -> Tuple[bool, str]: contextlib.nullcontext()), \ decorrelated_rollout_seed(repeat_idx - 1): ok, why = _validation_rollout() + repeat_seed = base_planner_seed + repeat_idx - 1 if ok: rollout_outcomes.append( - f"rollout {repeat_idx}: goal reached") + f"rollout {repeat_idx} (planner seed " + f"{repeat_seed}): goal reached") else: rollout_outcomes.append( - f"rollout {repeat_idx}: FAILED - {why}") + f"rollout {repeat_idx} (planner seed " + f"{repeat_seed}): FAILED - {why}") if flaky_detail is None: flaky_detail = (f"rollout {repeat_idx}/{n_rollouts} " + f"(planner seed {repeat_seed}) " f"FAILED: {why}") if flaky_detail is None: fresh_note = (", each on a freshly constructed simulator " "instance" if fresh_scope is not None else "") validation_note = ( - f" Validated {n_rollouts}/{n_rollouts} rollouts (the " + f" Validated {n_rollouts}/{n_rollouts} rollouts " + f"(planner seeds {base_planner_seed}-" + f"{base_planner_seed + n_rollouts - 1}; the " "simulator's motion planning and physics stepping vary " "across runs; repeats sample that execution " f"variability{fresh_note}).") + # Diagnostic trials: rollout_seed combined with + # validation_rollouts=N runs N rollouts at planner seeds + # S, S+1, ..., S+N-1 (rollout 1, reported step by step above, + # ran at S) - the same contract as explore_python's + # ``sim.run(plan, trials=N, seed=S)``. Reported only: a seeded + # run never captures and never arms the flaky escalation. + if (diagnostic_seed is not None and grounded_plan + and capped_request is not None and capped_request > 1): + r1_ok = (result.first_failure_idx is None and result.goal_reached) + if r1_ok: + r1_line = "goal reached" + elif result.first_failure_idx is not None: + r1_line = "FAILED - see the step report above" + else: + r1_line = "goal NOT reached" + diag_outcomes = [ + f"rollout 1 (planner seed {diagnostic_seed}): {r1_line}" + ] + for repeat_idx in range(2, capped_request + 1): + ctx.attempt_rollout_count += 1 + repeat_seed = diagnostic_seed + repeat_idx - 1 + with (fresh_scope() if fresh_scope is not None else + contextlib.nullcontext()), \ + absolute_rollout_seed(repeat_seed): + ok, why = _validation_rollout() + if ok: + diag_outcomes.append(f"rollout {repeat_idx} (planner seed " + f"{repeat_seed}): goal reached") + else: + diag_outcomes.append(f"rollout {repeat_idx} (planner seed " + f"{repeat_seed}): FAILED - {why}") + n_ok_diag = sum(1 for o in diag_outcomes + if o.endswith("goal reached")) + per_diag = "\n".join(f" {o}" for o in diag_outcomes) + lines.append( + f"Diagnostic trials: {n_ok_diag}/{capped_request} reached " + f"the goal (planner seeds {diagnostic_seed}-" + f"{diagnostic_seed + capped_request - 1}):\n{per_diag}") + # Physics-margin gate: the execution repeats above all run AT the # fitted physical params, so they cannot see a plan whose success # band excludes the fit's parameter error (run_20260723_091108: a @@ -505,7 +641,7 @@ def _validation_rollout() -> Tuple[bool, str]: and ctx.physics_margin_provider is not None and ctx.capture_goal_reaching_plans and is_current and goal_achieved and not evaluator_rejected and grounded_plan - and flaky_detail is None): + and diagnostic_seed is None and flaky_detail is None): for point in ctx.physics_margin_provider() or []: ctx.attempt_rollout_count += 1 with fresh_scope(physical_overrides=point): @@ -548,7 +684,11 @@ def _stash_uncaptured_submission() -> None: # also documents the best-effort-mode semantics); the branches # below apply its ctx mutations and format its messages. capture_outcome = _decide_capture( - capture_enabled=ctx.capture_goal_reaching_plans, + # A seeded diagnostic rollout is never captured: letting the + # agent choose the planner seed of a capturing rollout would + # let a cherry-picked known-good seed bypass the gate. + capture_enabled=(ctx.capture_goal_reaching_plans + and diagnostic_seed is None), is_current_task=is_current, have_plan=bool(grounded_plan), goal_achieved=goal_achieved, @@ -642,12 +782,16 @@ def _stash_uncaptured_submission() -> None: f"FLAKY (plan NOT captured): the plan reached the goal on " f"rollout 1 but {flaky_detail}. Per-rollout outcomes " f"(estimated reliability {n_ok}/{n_rollouts}):\n" - f" rollout 1: goal reached\n{per_rollout}\n" + f" rollout 1 (planner seed {base_planner_seed}): " + f"goal reached\n{per_rollout}\n" "The simulator's motion " "planning and physics stepping vary across runs, and the " "real environment samples the same variability - a plan " "that only sometimes succeeds in simulation will likely " - "fail for real. Add margin (e.g. tighter spacing, aim " + "fail for real. To debug a failed rollout first, re-run " + "it exactly: call this tool with rollout_seed= for full per-step reporting at " + "that seed. Then add margin (e.g. tighter spacing, aim " "impacts closer to the middle of the fall path) and " "resubmit. Because this task has now produced a flaky " f"submission, captures require {escalated_n}/{escalated_n} " diff --git a/tests/agent_sdk/test_evaluate_option_plan_capture.py b/tests/agent_sdk/test_evaluate_option_plan_capture.py index 62c6bbd67..2384e35eb 100644 --- a/tests/agent_sdk/test_evaluate_option_plan_capture.py +++ b/tests/agent_sdk/test_evaluate_option_plan_capture.py @@ -118,7 +118,7 @@ def _make_ctx(model, evaluator=None, best_effort=False, goal_nl=None): return ctx -def _call_tool(ctx, plan_text=_PLAN_TEXT): +def _call_tool(ctx, plan_text=_PLAN_TEXT, extra_args=None): """Invoke the real tool handler once against ``ctx``.""" tools = { t.name: t.handler @@ -129,10 +129,11 @@ def _call_tool(ctx, plan_text=_PLAN_TEXT): except RuntimeError: loop = asyncio.new_event_loop() asyncio.set_event_loop(loop) - result: Any = loop.run_until_complete(tools["evaluate_option_plan"]({ - "plan": - plan_text - })) + call_args = {"plan": plan_text} + if extra_args: + call_args.update(extra_args) + result: Any = loop.run_until_complete( + tools["evaluate_option_plan"](call_args)) return result["content"][0]["text"] @@ -141,13 +142,14 @@ def _run_tool(model, rollouts=3, plan_text=_PLAN_TEXT, best_effort=False, - goal_nl=None): + goal_nl=None, + extra_args=None): utils.reset_config({"agent_plan_validation_rollouts": rollouts}) ctx = _make_ctx(model, evaluator=evaluator, best_effort=best_effort, goal_nl=goal_nl) - return _call_tool(ctx, plan_text), ctx + return _call_tool(ctx, plan_text, extra_args=extra_args), ctx def test_robust_plan_is_captured_with_validation_note(): @@ -166,7 +168,7 @@ def test_flaky_plan_is_not_captured(): model = _Model(succeed_first_n=1) text, ctx = _run_tool(model, rollouts=3) assert "FLAKY (plan NOT captured)" in text - assert "rollout 2/3 FAILED" in text + assert "rollout 2/3 (planner seed" in text assert "goal not reached" in text assert "ReachedHi" in text assert "Captured as the current answer" not in text @@ -193,9 +195,11 @@ def test_flaky_message_reports_all_rollout_outcomes(): model = _Model(succeed_first_n=1) text, _ = _run_tool(model, rollouts=3) assert "estimated reliability 1/3" in text - assert "rollout 1: goal reached" in text - assert "rollout 2: FAILED" in text - assert "rollout 3: FAILED" in text + assert "rollout 1 (planner seed" in text + assert "): goal reached" in text + assert "rollout 2 (planner seed" in text + assert "rollout 3 (planner seed" in text + assert text.count("FAILED -") >= 2 # All three rollouts actually ran (no early break). assert model.num_calls == 3 @@ -550,7 +554,7 @@ def test_best_effort_flaky_plan_is_captured(): text, ctx = _run_tool(model, rollouts=3, best_effort=True) assert "Captured as the current answer" in text assert "best-effort" in text - assert "rollout 2/3 FAILED" in text + assert "rollout 2/3 (planner seed" in text assert "FLAKY (plan NOT captured)" not in text assert ctx.solved_plan is not None assert ctx.solved_plan_reached_goal is False @@ -632,3 +636,91 @@ def get_next_state_and_num_actions(self, state, option): assert ctx.solved_plan is not None # One capture rollout at the base seed, two decorrelated repeats. assert model.seeds == [base, base + 1, base + 2] + + +class _SeedRecordingModel2(_Model): + """Records ``CFG.seed`` at each rollout step (module-level reuse).""" + + def __init__(self, succeed_first_n=10**9): + super().__init__(succeed_first_n=succeed_first_n) + self.seeds = [] + + def get_next_state_and_num_actions(self, state, option): + from predicators.settings import \ + CFG # pylint: disable=import-outside-toplevel + self.seeds.append(CFG.seed) + return super().get_next_state_and_num_actions(state, option) + + +def test_validation_rollouts_arg_raises_the_gate(): + """``validation_rollouts=N`` requests a stricter gate than configured.""" + model = _Model() + text, ctx = _run_tool(model, + rollouts=3, + extra_args={"validation_rollouts": 5}) + assert "Validated 5/5 rollouts" in text + assert ctx.solved_plan is not None + assert model.num_calls == 5 + + +def test_validation_rollouts_arg_cannot_lower_the_gate(): + """A request below the configured gate is ignored: the gate is a floor - + letting the agent lower it would let a lucky draw bypass validation.""" + model = _Model() + text, ctx = _run_tool(model, + rollouts=3, + extra_args={"validation_rollouts": 1}) + assert "Validated 3/3 rollouts" in text + assert ctx.solved_plan is not None + assert model.num_calls == 3 + + +def test_flaky_report_names_seeds_and_reproduction_path(): + """A FLAKY rejection names each rollout's planner seed and tells the agent + how to reproduce the failed rollout (``rollout_seed``).""" + model = _Model(succeed_first_n=1) + text, _ = _run_tool(model, rollouts=3) + from predicators.settings import \ + CFG # pylint: disable=import-outside-toplevel + base = CFG.seed + assert f"rollout 1 (planner seed {base}): goal reached" in text + assert f"(planner seed {base + 1}): FAILED" in text + assert "rollout_seed=" in text + + +def test_rollout_seed_with_trials_runs_consecutive_seeds(): + """``rollout_seed=S`` + ``validation_rollouts=N`` runs N diagnostic + trials at planner seeds S..S+N-1 (mirroring ``sim.run(plan, trials=N, + seed=S)``), reports each with its seed, and still never captures.""" + model = _SeedRecordingModel2() + text, ctx = _run_tool(model, + rollouts=5, + extra_args={ + "rollout_seed": 900, + "validation_rollouts": 3 + }) + # Exactly the requested 3 trials - the configured gate (5) does not + # inflate a diagnostic run. + assert model.seeds == [900, 901, 902] + assert "Diagnostic trials: 3/3 reached the goal" in text + assert "rollout 2 (planner seed 901): goal reached" in text + assert "rollout 3 (planner seed 902): goal reached" in text + assert "Captured as the current answer" not in text + assert ctx.solved_plan is None + + +def test_rollout_seed_is_diagnostic_only(): + """A seeded rollout runs at exactly that planner seed, reports fully, and + is never captured - agent-chosen seeds must not pass the capture gate.""" + model = _SeedRecordingModel2() + text, ctx = _run_tool(model, rollouts=3, extra_args={"rollout_seed": 4242}) + assert "DIAGNOSTIC rollout at planner seed 4242" in text + assert model.seeds == [4242] # no validation repeats either + assert "Goal achieved: True" in text + assert "Captured as the current answer" not in text + assert "Validated" not in text + assert ctx.solved_plan is None + from predicators.settings import \ + CFG # pylint: disable=import-outside-toplevel + # The base seed is restored after the seeded rollout. + assert CFG.seed != 4242 From ef95b8f7866b9bfe3789a02c068490db508878e7 Mon Sep 17 00:00:00 2001 From: Yichao Liang Date: Wed, 19 Aug 2026 11:48:04 -0400 Subject: [PATCH 29/30] update agent SDK model name from "claude-sonnet-5" to "claude-opus-5" --- predicators/settings.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/predicators/settings.py b/predicators/settings.py index cf22456f0..9b5a1d837 100644 --- a/predicators/settings.py +++ b/predicators/settings.py @@ -1453,7 +1453,7 @@ class GlobalSettings: vlm_predicator_num_proposal_batches = 1 # agent SDK online abstraction learning parameters - agent_sdk_model_name = "claude-sonnet-5" + agent_sdk_model_name = "claude-opus-5" agent_sdk_max_agent_turns_per_iteration = 50 # Consecutive agent queries that die without the agent doing ANY work # (an auth/billing banner as the only assistant text, an error result, From 9f44c7e4943709a65a7ec96fb69fefc837a596cd Mon Sep 17 00:00:00 2001 From: Yichao Liang Date: Thu, 20 Aug 2026 16:20:27 -0400 Subject: [PATCH 30/30] style: isort and docformatter conformance for the capture tests Import-order fix plus docstring reflows; one docstring docformatter would split mid-sentence is reworded into a proper summary and body. --- .../agent_sdk/test_evaluate_option_plan_capture.py | 14 +++++++++----- 1 file changed, 9 insertions(+), 5 deletions(-) diff --git a/tests/agent_sdk/test_evaluate_option_plan_capture.py b/tests/agent_sdk/test_evaluate_option_plan_capture.py index 2384e35eb..a556d44b8 100644 --- a/tests/agent_sdk/test_evaluate_option_plan_capture.py +++ b/tests/agent_sdk/test_evaluate_option_plan_capture.py @@ -689,9 +689,9 @@ def test_flaky_report_names_seeds_and_reproduction_path(): def test_rollout_seed_with_trials_runs_consecutive_seeds(): - """``rollout_seed=S`` + ``validation_rollouts=N`` runs N diagnostic - trials at planner seeds S..S+N-1 (mirroring ``sim.run(plan, trials=N, - seed=S)``), reports each with its seed, and still never captures.""" + """``rollout_seed=S`` + ``validation_rollouts=N`` runs N diagnostic trials + at planner seeds S..S+N-1 (mirroring ``sim.run(plan, trials=N, seed=S)``), + reports each with its seed, and still never captures.""" model = _SeedRecordingModel2() text, ctx = _run_tool(model, rollouts=5, @@ -710,8 +710,11 @@ def test_rollout_seed_with_trials_runs_consecutive_seeds(): def test_rollout_seed_is_diagnostic_only(): - """A seeded rollout runs at exactly that planner seed, reports fully, and - is never captured - agent-chosen seeds must not pass the capture gate.""" + """A seeded rollout runs at exactly the given planner seed. + + It reports fully and is never captured - agent-chosen seeds must + not pass the capture gate. + """ model = _SeedRecordingModel2() text, ctx = _run_tool(model, rollouts=3, extra_args={"rollout_seed": 4242}) assert "DIAGNOSTIC rollout at planner seed 4242" in text @@ -722,5 +725,6 @@ def test_rollout_seed_is_diagnostic_only(): assert ctx.solved_plan is None from predicators.settings import \ CFG # pylint: disable=import-outside-toplevel + # The base seed is restored after the seeded rollout. assert CFG.seed != 4242