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
4 changes: 4 additions & 0 deletions agent_context/topics/task-programs/execution.md
Original file line number Diff line number Diff line change
Expand Up @@ -216,6 +216,10 @@ symbolic state only when it declares no effect contract. Declaring a held
effect or phase does not certify physical attachment or weaken the selected
assurance; verified acquisition still requires its measured monitor. A release
or transfer target contributes to pickup look-ahead and then ends that chain.
A look-ahead target also records the world-z `approach_clearance` from which
Place reaches it (its `lift_height`, unless `max_approach_retract_z` caps it).
A pickup whose options set `screen_downstream_approach` screens that raised
pose before the target; by default only the target is screened.

## MLLM boundary

Expand Down
47 changes: 47 additions & 0 deletions 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."""

screen_downstream_approach: bool = field(default=False, kw_only=True)
"""Also require the pose from which a downstream placement approaches.

Place reaches its target through a pose ``lift_height`` above it. With this
option the semantic compiler adds that raised object pose to
``downstream_object_target_poses``, so a grasp is selected only when the
arm can reach both. Without it only the target itself is screened.

The IK solver converges only from a nearby seed, so with this option a
downstream pose that has no solution from the pose before it is also tried
from the lift pose and from the pose that follows it.
"""

def __post_init__(self) -> None:
if (
isinstance(self.initial_lift_height, bool)
Expand Down Expand Up @@ -285,6 +298,8 @@ def __post_init__(self) -> None:
_validate_single_se3(self.grasp_frame_to_eef, "grasp_frame_to_eef")
if type(self.cartesian_lift) is not bool:
raise TypeError("cartesian_lift must be boolean.")
if type(self.screen_downstream_approach) is not bool:
raise TypeError("screen_downstream_approach must be boolean.")
if type(self.approach_along_grasp_axis) is not bool:
raise TypeError("approach_along_grasp_axis must be boolean.")
if self.approach_along_grasp_axis and self.fixed_object_to_eef is None:
Expand Down Expand Up @@ -1022,6 +1037,8 @@ def _select_feasible_grasp_variants(
# MoveHeldObject begins after the lift, so screen its target from the
# same joint state that the execution stream will use.
downstream_seed = lift_qpos
downstream_poses: list[torch.Tensor] = []
downstream_results: list[tuple[torch.Tensor, torch.Tensor]] = []
for object_target_pose in options.downstream_object_target_poses:
object_target_pose = object_target_pose.to(
device=self.device, dtype=torch.float32
Expand All @@ -1042,6 +1059,36 @@ def _select_feasible_grasp_variants(
downstream_success, downstream_seed = self._compute_batch_candidate_ik(
downstream_eef_variants, downstream_seed, manipulator
)
if options.screen_downstream_approach and not downstream_success.all():
# The solver converges from a nearby seed only. A pose it
# misses from the previous pose is tried from the lift pose.
retried, retried_qpos = self._compute_batch_candidate_ik(
downstream_eef_variants, lift_qpos, manipulator
)
recovered = retried & ~downstream_success
downstream_seed = torch.where(
recovered[..., None], retried_qpos, downstream_seed
)
downstream_success = downstream_success | retried
downstream_poses.append(downstream_eef_variants)
downstream_results.append((downstream_success, downstream_seed))
if options.screen_downstream_approach:
# ... and then from the pose that follows it, so an approach pose
# can be reached by continuity from its target as well.
for index in range(len(downstream_poses) - 2, -1, -1):
success, qpos = downstream_results[index]
following_success, following_qpos = downstream_results[index + 1]
if not (~success & following_success).any():
continue
retried, retried_qpos = self._compute_batch_candidate_ik(
downstream_poses[index], following_qpos, manipulator
)
recovered = retried & ~success & following_success
downstream_results[index] = (
success | recovered,
torch.where(recovered[..., None], retried_qpos, qpos),
)
for downstream_success, _ in downstream_results:
pickup_success &= downstream_success
downstream_success_counts.append(pickup_success.sum(dim=(1, 2)).tolist())
if not pickup_success.any(dim=(1, 2)).all():
Expand Down
9 changes: 9 additions & 0 deletions embodichain/lab/sim/atomic_skills/primitives/place.py
Original file line number Diff line number Diff line change
Expand Up @@ -153,6 +153,15 @@ class PlaceOptions(SkillOptions):
preserve_current_object_orientation: bool = False
"""Keep the held object's observed world orientation at the place target."""

@property
def approach_clearance(self) -> float:
"""World-z distance (m) of the approach pose above the release pose.

Zero when ``max_approach_retract_z`` caps the approach, because the
capped height then depends on the release pose itself.
"""
return 0.0 if self.max_approach_retract_z is not None else self.lift_height

def __post_init__(self) -> None:
if type(self.cartesian_approach) is not bool:
raise TypeError("cartesian_approach must be boolean.")
Expand Down
38 changes: 38 additions & 0 deletions embodichain/lab/task_program/compiler/lowering.py
Original file line number Diff line number Diff line change
Expand Up @@ -23,6 +23,7 @@
from copy import deepcopy
from dataclasses import dataclass, field, replace
from enum import Enum
import math
from types import MappingProxyType
from typing import ClassVar, TypeVar
from uuid import uuid4
Expand Down Expand Up @@ -237,11 +238,15 @@ class SemanticObjectTarget:
an explicitly installed typed/versioned grounder. Orientation preservation
records the downstream placement policy so pickup feasibility checks use
the same object target that placement will eventually lower.
``approach_clearance`` is the world-z distance above the target from which
the downstream call approaches it; a pickup that screens downstream
approaches also requires that raised pose to be reachable.
"""

pose: SemanticPose | SemanticRelativePose | SceneEntityPose | None = None
relation: SemanticRelationTarget | None = None
preserve_current_object_orientation: bool = False
approach_clearance: float = 0.0

def __post_init__(self) -> None:
selected = sum(value is not None for value in (self.pose, self.relation))
Expand All @@ -264,6 +269,29 @@ def __post_init__(self) -> None:
raise TypeError("relation must be exactly SemanticRelationTarget or None.")
if type(self.preserve_current_object_orientation) is not bool:
raise TypeError("preserve_current_object_orientation must be a bool.")
if (
isinstance(self.approach_clearance, bool)
or not isinstance(self.approach_clearance, (int, float))
or not math.isfinite(self.approach_clearance)
or self.approach_clearance < 0.0
):
raise ValueError("approach_clearance must be finite and non-negative.")


def _raised_pose_goal(target: PoseGoalValue, height: float) -> PoseGoalValue:
"""Return a pose goal translated by ``height`` along world z.

A late-bound scene pose stays late-bound: the offset is added to its world
displacement.
"""
if isinstance(target, torch.Tensor):
raised = target.clone()
raised[..., 2, 3] += height
return raised
offset = torch.tensor([0.0, 0.0, height], dtype=torch.float32)
if target.world_displacement is not None:
offset = target.world_displacement + offset.to(target.world_displacement)
return replace(target, world_displacement=offset)


@dataclass(frozen=True, slots=True)
Expand Down Expand Up @@ -1966,6 +1994,7 @@ def _downstream_targets(
preserve_current_object_orientation=(
place_options.preserve_current_object_orientation
),
approach_clearance=place_options.approach_clearance,
)
)
break
Expand Down Expand Up @@ -2026,6 +2055,15 @@ def _lower_pick(
object_id=call.object.entity_id,
context=context,
)
if (
option_template.screen_downstream_approach
and target.approach_clearance > 0.0
):
# The downstream call reaches the target through the pose
# above it, so that pose is visited, and screened, first.
downstream_targets.append(
_raised_pose_goal(grounded_target, target.approach_clearance)
)
downstream_targets.append(grounded_target)
return SemanticLowering(
goal=GraspGoal(semantics=semantics),
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -1009,6 +1009,7 @@ def pick_lookahead_targets(
preserve_current_object_orientation=(
options.preserve_current_object_orientation
),
approach_clearance=options.approach_clearance,
),
)

Expand Down
7 changes: 7 additions & 0 deletions embodichain/lab/task_program/integrations/configured.py
Original file line number Diff line number Diff line change
Expand Up @@ -1105,6 +1105,7 @@ def _decode_skill_options(value: object, *, path: str) -> SkillOptions:
"max_approach_retract_z",
"cartesian_waypoint_count",
"cartesian_approach",
"screen_downstream_approach",
"preserve_current_object_orientation",
"direction",
"approach_distance",
Expand Down Expand Up @@ -1162,6 +1163,7 @@ def _decode_skill_options(value: object, *, path: str) -> SkillOptions:
"approach_along_grasp_axis",
"initial_lift_height",
"cartesian_lift",
"screen_downstream_approach",
)
if kind == "pick_up"
else ()
Expand All @@ -1175,6 +1177,11 @@ def _decode_skill_options(value: object, *, path: str) -> SkillOptions:
kwargs["cartesian_lift"] = _boolean(
config["cartesian_lift"], path=f"{path}.cartesian_lift"
)
if "screen_downstream_approach" in config:
kwargs["screen_downstream_approach"] = _boolean(
config["screen_downstream_approach"],
path=f"{path}.screen_downstream_approach",
)
if "approach_along_grasp_axis" in config:
kwargs["approach_along_grasp_axis"] = _boolean(
config["approach_along_grasp_axis"],
Expand Down
9 changes: 9 additions & 0 deletions tests/gym/envs/task_program/test_configured_integration.py
Original file line number Diff line number Diff line change
Expand Up @@ -1314,6 +1314,15 @@ def test_relative_place_uses_fresh_reference_pose_and_verified_grasp() -> None:
torch.tensor((0.18, 0.0, 0.02)),
)
assert lookahead[0].preserve_current_object_orientation
assert lookahead[0].approach_clearance == options.lift_height
from embodichain.lab.task_program.compiler.lowering import _raised_pose_goal

raised = _raised_pose_goal(lookahead[0].pose, lookahead[0].approach_clearance)
assert type(raised) is SceneEntityPose and raised.entity_id == "notebook"
torch.testing.assert_close(
raised.world_displacement,
torch.tensor((0.18, 0.0, 0.02 + options.lift_height)),
)


def test_relative_place_rejects_unmigrated_local_service_kind() -> None:
Expand Down
44 changes: 44 additions & 0 deletions tests/lab/task_program/test_semantic_compiler.py
Original file line number Diff line number Diff line change
Expand Up @@ -2076,6 +2076,50 @@ def test_pick_lookahead_uses_downstream_place_orientation_policy() -> None:
torch.testing.assert_close(downstream[:, :3, :3], object_pose[:, :3, :3])


@pytest.mark.parametrize(
("screen", "max_approach_retract_z", "heights"),
[
(False, None, [0.3]),
# The pose above the target comes first, as Place visits it first.
(True, None, [0.4, 0.3]),
# A capped approach height is not an object-space clearance.
(True, 1.0, [0.3]),
],
)
def test_pick_lookahead_screens_the_downstream_place_approach_on_request(
screen: bool, max_approach_retract_z: float | None, heights: list[float]
) -> None:
registry, _ = _scene_registry()
templates = _skill_option_templates()
templates["pick"] = PickUpOptions(screen_downstream_approach=screen)
templates["place"] = PlaceOptions(
lift_height=0.1, max_approach_retract_z=max_approach_retract_z
)
compiler, _ = _compiler(
registry,
profile=_profile(
preset=_preset("safe", skill_option_templates=templates),
),
)
workflow = compiler.analyze(
(
Pick(object=SceneObjectRef("cube")),
Place(
object=SceneObjectRef("cube"),
at=SemanticPose((0.4, 0.2, 0.3), (0.0, 0.0, 0.0, 1.0)),
),
)
)

options = compiler.ground(workflow, 0, _context(registry)).invocation.skill_options

assert type(options) is PickUpOptions
assert [
float(pose[..., 2, 3].reshape(-1)[0])
for pose in options.downstream_object_target_poses
] == pytest.approx(heights)


def test_grounded_safe_invocation_requires_registered_dynamic_collision() -> None:
registry, _ = _scene_registry(dynamic_collision=True)
profile = _profile(
Expand Down
50 changes: 50 additions & 0 deletions tests/sim/atomic_skills/test_actions.py
Original file line number Diff line number Diff line change
Expand Up @@ -4318,6 +4318,56 @@ def feasible_ik(
assert torch.equal(aligned, expected)


@pytest.mark.parametrize("screen", [False, True])
def test_pick_downstream_screen_retries_from_neighbouring_seeds(screen: bool) -> None:
"""A pose missed from the previous pose is tried from the lift and the next pose."""
generator = _motion_generator()
action = _bind_action(generator, PickUp())
approach, target = torch.eye(4), torch.eye(4)
approach[2, 3] = 0.1
shape = (NUM_ENVS, 1, 2)
solved = torch.ones(shape, dtype=torch.bool)
lift_qpos = torch.full((*shape, ARM_DOF), 0.3)
target_qpos = torch.full((*shape, ARM_DOF), 0.7)
zeros = torch.zeros(*shape, ARM_DOF)
results = [
(solved, zeros), # pre-grasp
(solved, zeros), # grasp
(solved, lift_qpos), # lift
(~solved, zeros), # approach pose from the lift pose
*([(~solved, zeros)] if screen else []), # ... retried from the lift pose
(~solved, zeros), # target from the unsolved approach pose
*(
[
(solved, target_qpos), # target from the lift pose
(solved, zeros), # approach pose from the target
]
if screen
else []
),
]
action._compute_batch_candidate_ik = Mock(side_effect=results)

_, ik_success = action._select_feasible_grasp_variants(
torch.eye(4).repeat(NUM_ENVS, 1, 1, 1),
torch.zeros(NUM_ENVS, ARM_DOF),
torch.eye(4).repeat(NUM_ENVS, 1, 1),
JointPositionTarget("arm", tuple(range(ARM_DOF))),
PickUpOptions(
downstream_object_target_poses=(approach, target),
screen_downstream_approach=screen,
),
torch.tensor((0.0, 0.0, -1.0)),
)

calls = action._compute_batch_candidate_ik.call_args_list
assert len(calls) == len(results)
assert bool(ik_success.all()) is screen
if screen:
torch.testing.assert_close(calls[6].args[1], lift_qpos)
torch.testing.assert_close(calls[7].args[1], target_qpos)


def test_pick_resolves_late_bound_scene_grasp_and_declares_dependency() -> None:
generator = _motion_generator()
target_pose = torch.eye(4).repeat(NUM_ENVS, 1, 1)
Expand Down
Loading