Skip to content
Open
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
6 changes: 6 additions & 0 deletions agent_context/topics/atomic-actions/execution.md
Original file line number Diff line number Diff line change
Expand Up @@ -253,6 +253,12 @@ The runner cancels/holds safely before its once-only abort notification.

## Contact-path selection

PickUp samples grasps for one world approach direction and requires pre-grasp,
grasp, lift and downstream targets to be reachable. Its optional
`approach_tilts` also screen the approach rotated outward, from the arm base
toward the object, by each angle; a row uses the direction that leaves the most
reachable candidates, the configured one on ties. The default is unchanged.

Pickup's optional `cartesian_lift` and placement's `cartesian_approach` use the
existing sampled Cartesian path and joint-limit retiming with `ik_interp`.
Pickup plans a separate straight world-Z lift only after a feasible approach;
Expand Down
125 changes: 124 additions & 1 deletion embodichain/lab/sim/atomic_skills/primitives/pick_up.py
Original file line number Diff line number Diff line change
Expand Up @@ -228,6 +228,19 @@ class PickUpOptions(SkillOptions):
initial_lift_height: float = field(default=0.0, kw_only=True)
"""Initial world-Z clearance (m) from the current TCP pose before approach."""

approach_tilts: tuple[float, ...] = field(default=(), kw_only=True)
"""Outward tilt angles (rad) of the approach direction to consider as well.

A short arm cannot reach far objects with a vertical gripper. Each angle
rotates the approach direction outward, toward the horizontal direction
from the arm base to the object, and repeats grasp sampling and the
reachability screen (pre-grasp, grasp, lift and downstream targets). Each
row uses the direction that leaves the most reachable grasp candidates;
``approach_direction`` wins ties, then the smaller angle. Explicit goal
grasps and ``fixed_object_to_eef`` bypass sampling and therefore this
option.
"""

def __post_init__(self) -> None:
if (
isinstance(self.initial_lift_height, bool)
Expand Down Expand Up @@ -273,6 +286,13 @@ def __post_init__(self) -> None:
torch.linalg.vector_norm(self.approach_direction.float(), dim=-1) <= 1.0e-6
).any():
raise ValueError("approach_direction rows must be non-zero.")
if not isinstance(self.approach_tilts, tuple) or any(
isinstance(angle, bool)
or not isinstance(angle, (int, float))
or not 0.0 < angle < math.pi / 2
for angle in self.approach_tilts
):
raise ValueError("approach_tilts must be a tuple of angles in (0, pi / 2).")
if self.approach_alignment_max_angle is not None and not (
0.0 <= self.approach_alignment_max_angle <= math.pi / 2
):
Expand Down Expand Up @@ -627,7 +647,10 @@ def _plan(
affordance_sample: AffordanceSample | None = None
if target.grasp_xpos is None:
if options.fixed_object_to_eef is None:
affordance_sample = self._resolve_grasp_pose(
(
affordance_sample,
approach_direction,
) = self._resolve_grasp_pose_with_tilts(
sem,
object_pose,
start_arm_qpos,
Expand Down Expand Up @@ -906,6 +929,106 @@ def _assemble_pick_plan(
),
)

def _resolve_grasp_pose_with_tilts(
self,
semantics: ObjectSemantics,
object_pose: torch.Tensor,
start_qpos: torch.Tensor,
manipulator: JointPositionTarget,
grasp_target_id: str,
options: PickUpOptions,
approach_direction: torch.Tensor,
context: PlanningContext,
*,
sample_key: str,
) -> tuple[AffordanceSample, torch.Tensor]:
"""Resolve a grasp along the most reachable configured approach.

Returns:
The grasp sample and the approach direction each row uses. Both are
those of ``approach_direction`` unless a tilted approach leaves
more reachable grasp candidates.
"""

def resolve(direction: torch.Tensor) -> tuple[AffordanceSample, torch.Tensor]:
sample = self._resolve_grasp_pose(
semantics,
object_pose,
start_qpos,
manipulator,
grasp_target_id,
options,
direction,
context,
sample_key=sample_key,
)
reachable = sample.metadata.get("valid_candidate_counts")
counts = (
torch.as_tensor(reachable, device=self.device)
if isinstance(reachable, list) and len(reachable) == self.num_envs
else sample.success.to(torch.long)
)
return sample, torch.where(sample.success, counts, 0)

sample, counts = resolve(approach_direction)
if not options.approach_tilts:
return sample, approach_direction

directions = approach_direction.expand(self.num_envs, 3).clone()
base_pose = self.robot.get_control_part_base_pose(
name=manipulator.control_part,
to_matrix=True,
**(
{"env_ids": self.planning_services.env_ids}
if self.planning_services.env_ids is not None
else {}
),
).to(device=self.device, dtype=directions.dtype)
outward = object_pose[:, :3, 3].to(directions.dtype) - base_pose[:, :3, 3]
outward = outward - (outward * directions).sum(-1, keepdim=True) * directions
length = torch.linalg.vector_norm(outward, dim=-1, keepdim=True)
tiltable = length.squeeze(-1) > 1.0e-6
outward = outward / length.clamp_min(1.0e-6)

configured = directions.clone()
success = sample.success.clone()
poses = sample.poses.clone()
metadata = dict(sample.metadata)
tilts = torch.zeros(self.num_envs, device=self.device)
for angle in sorted(options.approach_tilts):
if not tiltable.any():
break
tilted = torch.where(
tiltable[:, None],
math.cos(angle) * configured + math.sin(angle) * outward,
configured,
)
attempt, attempt_counts = resolve(tilted)
better = tiltable & (attempt_counts > counts)
if not better.any():
continue
poses[better] = attempt.poses[better]
directions[better] = tilted[better]
tilts[better] = angle
counts[better] = attempt_counts[better]
success |= better
for key, rows in attempt.metadata.items():
current = metadata.get(key)
if (
isinstance(rows, list)
and isinstance(current, list)
and len(rows) == len(current) == self.num_envs
):
metadata[key] = [
rows[row] if better[row] else current[row]
for row in range(self.num_envs)
]
metadata["approach_tilts"] = tilts.cpu().tolist()
return (
AffordanceSample(success=success, poses=poses, metadata=metadata),
directions,
)

def _resolve_grasp_pose(
self,
semantics: ObjectSemantics,
Expand Down
15 changes: 15 additions & 0 deletions embodichain/lab/task_program/integrations/configured.py
Original file line number Diff line number Diff line change
Expand Up @@ -1098,6 +1098,7 @@ def _decode_skill_options(value: object, *, path: str) -> SkillOptions:
"middle_empty_ratio",
"grasp_seed",
"approach_alignment_max_angle",
"approach_tilts",
"obj_upright_direction",
"rotate_upright",
"grasp_frame_to_eef",
Expand Down Expand Up @@ -1160,6 +1161,7 @@ def _decode_skill_options(value: object, *, path: str) -> SkillOptions:
*(
(
"approach_along_grasp_axis",
"approach_tilts",
"initial_lift_height",
"cartesian_lift",
)
Expand Down Expand Up @@ -1222,6 +1224,19 @@ def _decode_skill_options(value: object, *, path: str) -> SkillOptions:
),
dtype=torch.float32,
)
if "approach_tilts" in config:
angles = config["approach_tilts"]
if type(angles) is not list:
raise TypeError(f"{path}.approach_tilts must be a list.")
kwargs["approach_tilts"] = tuple(
_real(
angle,
path=f"{path}.approach_tilts[{index}]",
minimum=0.0,
maximum=math.pi / 2,
)
for index, angle in enumerate(angles)
)
if "approach_alignment_max_angle" in config:
kwargs["approach_alignment_max_angle"] = _real(
config["approach_alignment_max_angle"],
Expand Down
57 changes: 57 additions & 0 deletions tests/sim/atomic_skills/test_actions.py
Original file line number Diff line number Diff line change
Expand Up @@ -3997,6 +3997,63 @@ def test_pick_holds_only_environment_without_a_feasible_grasp() -> None:
assert held.env_mask.tolist() == [True, False]


def test_pick_uses_the_approach_tilt_with_the_most_reachable_grasps() -> None:
generator = _motion_generator()
action = _bind_action(generator, PickUp())
angle = math.radians(25.0)
vertical = torch.tensor([0.0, 0.0, -1.0])
tilted_pose = torch.eye(4).repeat(NUM_ENVS, 1, 1)
tilted_pose[:, 0, 3] = 0.5
# Row 0 keeps more reachable grasps along the configured direction; row 1
# has two there and nine with the tilt.
action._resolve_grasp_pose = Mock(
side_effect=[
AffordanceSample(
success=torch.tensor([True, True]),
poses=torch.eye(4).repeat(NUM_ENVS, 1, 1),
metadata={"candidate_ids": [3, 4], "valid_candidate_counts": [8, 2]},
),
AffordanceSample(
success=torch.tensor([True, True]),
poses=tilted_pose,
metadata={"candidate_ids": [7, 9], "valid_candidate_counts": [8, 9]},
),
]
)
# Arm base at the origin, object one metre along +x.
action.robot.get_control_part_base_pose = Mock(
return_value=torch.eye(4).repeat(NUM_ENVS, 1, 1)
)
object_pose = torch.eye(4).repeat(NUM_ENVS, 1, 1)
object_pose[:, 0, 3] = 1.0

sample, directions = action._resolve_grasp_pose_with_tilts(
ObjectSemantics(
affordance=AntipodalAffordance(),
geometry={},
label="object",
entity_id="target",
),
object_pose,
torch.zeros(NUM_ENVS, ARM_DOF),
JointPositionTarget("arm", tuple(range(ARM_DOF))),
"gripper",
PickUpOptions(approach_tilts=(angle,)),
vertical,
_context(),
sample_key="grasp",
)

outward = torch.tensor([math.sin(angle), 0.0, -math.cos(angle)])
retried = action._resolve_grasp_pose.call_args_list[1].args[6]
torch.testing.assert_close(retried, outward.expand(NUM_ENVS, 3))
torch.testing.assert_close(directions, torch.stack([vertical, outward]))
assert sample.poses[:, 0, 3].tolist() == [0.0, 0.5]
assert sample.metadata["candidate_ids"] == [3, 9]
assert sample.metadata["valid_candidate_counts"] == [8, 9]
assert sample.metadata["approach_tilts"] == pytest.approx([0.0, angle])


def test_pick_normalizes_integer_batch_ik_success_masks() -> None:
"""DexSim integer IK flags become boolean masks before tensor selection."""
generator = _motion_generator()
Expand Down
Loading