Skip to content
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 1 addition & 1 deletion predicators/ground_truth_models/fan/processes.py
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
15 changes: 13 additions & 2 deletions predicators/ground_truth_models/skill_factories/push.py
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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],
Expand Down Expand Up @@ -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"),
Expand Down
6 changes: 6 additions & 0 deletions predicators/pybullet_helpers/robots/panda.py
Original file line number Diff line number Diff line change
Expand Up @@ -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(
Expand Down
12 changes: 12 additions & 0 deletions predicators/pybullet_helpers/robots/single_arm.py
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
13 changes: 3 additions & 10 deletions predicators/settings.py
Original file line number Diff line number Diff line change
Expand Up @@ -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]
Expand Down
5 changes: 5 additions & 0 deletions tests/pybullet_helpers/robots/test_panda.py
Original file line number Diff line number Diff line change
Expand Up @@ -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)
22 changes: 21 additions & 1 deletion tests/test_skill_factories.py
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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."""
Expand Down
Loading