diff --git a/predicators/ground_truth_models/fan/processes.py b/predicators/ground_truth_models/fan/processes.py index ded8c52e2..dd0e3eac4 100644 --- a/predicators/ground_truth_models/fan/processes.py +++ b/predicators/ground_truth_models/fan/processes.py @@ -19,7 +19,7 @@ def _push_sampler(state: State, goal: Set[GroundAtom], """Return fixed push params for fan switch push. Approach 0.075 sits in a measured window: with the side-oriented - gripper (default skill_push_ee_yaw_offset 0) the hand extends along + gripper (the fetch's push_ee_yaw_offset, 0) the hand extends along the approach axis, so below ~0.073 the descend waypoint collides with an end-of-row switch (BiRRT goal-in-collision), while at 0.08 the far-side (Off-push) waypoint already stalls at the fetch arm's diff --git a/predicators/ground_truth_models/skill_factories/push.py b/predicators/ground_truth_models/skill_factories/push.py index 871a7d9bd..368978570 100644 --- a/predicators/ground_truth_models/skill_factories/push.py +++ b/predicators/ground_truth_models/skill_factories/push.py @@ -59,7 +59,7 @@ def _get_domino_pose(state, objects, params, config): from predicators.structs import Array, Object, ParameterizedOption, State, Type # Canonical continuous parameters for Push. The approach upper bound -# leaves room for the side-oriented gripper (skill_push_ee_yaw_offset 0), +# leaves room for a side-oriented gripper (yaw offset 0), # whose body extends along the approach axis: a descend waypoint closer # than ~0.07 m can itself collide with the pushed object. That caveat is # stated in the description because the tool-facing params text is the @@ -75,6 +75,17 @@ def _get_domino_pose(state, objects, params, config): ] +def resolve_ee_yaw_offset(config: SkillConfig) -> float: + """The EE yaw offset Push should use, in radians. + + Which face of the gripper leads into the object is a property of the + hand, so it comes from the robot unless the config forces one. + """ + if CFG.skill_push_ee_yaw_offset is None: + return config.robot.push_ee_yaw_offset + return float(CFG.skill_push_ee_yaw_offset) + + def create_push_skill( name: str, types: Sequence[Type], @@ -133,7 +144,7 @@ def _waypoints( push_xy = obj_xy home_xy = np.array(cfg.robot_home_pos[:2]) home_z = cfg.robot_home_pos[2] - ee_yaw = oyaw + CFG.skill_push_ee_yaw_offset + ee_yaw = oyaw + resolve_ee_yaw_offset(cfg) return [ (*behind_xy, cfg.transport_z, ee_yaw, "closed"), (*behind_xy, oz + s_offset_z, ee_yaw, "closed"), diff --git a/predicators/pybullet_helpers/robots/panda.py b/predicators/pybullet_helpers/robots/panda.py index a46027be2..3b1efd48e 100644 --- a/predicators/pybullet_helpers/robots/panda.py +++ b/predicators/pybullet_helpers/robots/panda.py @@ -84,6 +84,12 @@ def closed_fingers(self) -> float: return float(CFG.pybullet_closed_fingers) return 0.03 + @property + def push_ee_yaw_offset(self) -> float: + # The Franka Hand leads with its knuckles spanning the + # object's face. + return np.pi / 2 + @classmethod def ikfast_info(cls) -> Optional[IKFastInfo]: return IKFastInfo( diff --git a/predicators/pybullet_helpers/robots/single_arm.py b/predicators/pybullet_helpers/robots/single_arm.py index 7f4d7123d..c021a4c35 100644 --- a/predicators/pybullet_helpers/robots/single_arm.py +++ b/predicators/pybullet_helpers/robots/single_arm.py @@ -237,6 +237,18 @@ def closed_fingers(self) -> float: """The value at which the finger joints should be closed.""" raise NotImplementedError("Override me!") + @property + def push_ee_yaw_offset(self) -> float: + """End-effector yaw during Push, relative to the pushed object's yaw. + + Defaults to 0.0, which is what the Fetch hand was verified with + on domino chains and fan/boil switches (2026-07-14). A gripper + whose geometry differs overrides this. + ``CFG.skill_push_ee_yaw_offset`` overrides it for everyone when + set, for experiments. + """ + return 0.0 + @classmethod def home_arm_joint_positions(cls) -> Optional[List[float]]: """The robot's canonical home configuration (arm joints only, no diff --git a/predicators/settings.py b/predicators/settings.py index 92cf6bbd4..bd65df9f4 100644 --- a/predicators/settings.py +++ b/predicators/settings.py @@ -463,16 +463,9 @@ class GlobalSettings: # skill phase parameters skill_phase_use_motion_planning = False - # EE yaw relative to the pushed object's yaw during Push. The default - # (0.0, "side push") leads with the gripper's narrow side, shrinking - # the swept width so the stroke and retreat are less likely to clip - # neighbors (verified equivalent to the legacy front push on domino - # chains and fan/boil switches, 2026-07-14). pi/2 restores the legacy - # front push (knuckles spanning the object's face). Side push makes - # the hand longer ALONG the approach axis, so pushes need a real - # approach distance (a zero-length stroke cannot drag a switch, and - # a too-small offset can put the descend waypoint in collision). - skill_push_ee_yaw_offset = 0.0 + # EE yaw relative to the pushed object's yaw during Push. None (the + # default) takes it from the robot. + skill_push_ee_yaw_offset = None # coffee env parameters coffee_num_cups_train = [1, 2] diff --git a/tests/pybullet_helpers/robots/test_panda.py b/tests/pybullet_helpers/robots/test_panda.py index 1722fccde..f6c9f13aa 100644 --- a/tests/pybullet_helpers/robots/test_panda.py +++ b/tests/pybullet_helpers/robots/test_panda.py @@ -182,3 +182,8 @@ def test_panda_home_keeps_canonical_arm_under_rolled_orientation( assert np.allclose(panda.get_state()[:3], rolled_home_pose.position, atol=1e-3) + + +def test_panda_pushes_with_its_front_face(panda): + """The Franka Hand pushes front-on, unlike the base class's default.""" + assert panda.push_ee_yaw_offset == pytest.approx(np.pi / 2) diff --git a/tests/test_skill_factories.py b/tests/test_skill_factories.py index e84a5a07c..b692de8c9 100644 --- a/tests/test_skill_factories.py +++ b/tests/test_skill_factories.py @@ -20,7 +20,7 @@ from predicators.ground_truth_models.skill_factories.place import \ create_place_skill from predicators.ground_truth_models.skill_factories.push import \ - create_push_skill + create_push_skill, resolve_ee_yaw_offset from predicators.ground_truth_models.skill_factories.wait import \ create_wait_option from predicators.pybullet_helpers.geometry import Pose @@ -1153,6 +1153,26 @@ def test_push_policy_close_fingers_returns_valid_action(self, robot_scene): assert isinstance(action, Action) assert robot.action_space.contains(action.arr) + def test_ee_yaw_offset_comes_from_the_robot(self, robot_scene): + """With no config override, the hand decides the push orientation.""" + _, robot = robot_scene + utils.reset_config({"seed": 123, "skill_push_ee_yaw_offset": None}) + config = self._make_push_config(robot) + # The fetch pushes with the 0.0 default. + assert resolve_ee_yaw_offset(config) == robot.push_ee_yaw_offset == 0.0 + + def test_ee_yaw_offset_config_override_wins(self, robot_scene): + """Setting the flag forces one offset regardless of the robot.""" + _, robot = robot_scene + utils.reset_config({ + "seed": 123, + "skill_push_ee_yaw_offset": np.pi / 2 + }) + config = self._make_push_config(robot) + assert resolve_ee_yaw_offset(config) == pytest.approx(np.pi / 2) + assert robot.push_ee_yaw_offset == 0.0 + utils.reset_config({"seed": 123}) + def test_fmt_option_params(): """Params render compactly for failure messages, including empty."""