diff --git a/agent_context/topics/task-programs/execution.md b/agent_context/topics/task-programs/execution.md index 113df477f..ad465c5bb 100644 --- a/agent_context/topics/task-programs/execution.md +++ b/agent_context/topics/task-programs/execution.md @@ -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 diff --git a/embodichain/lab/sim/atomic_skills/primitives/pick_up.py b/embodichain/lab/sim/atomic_skills/primitives/pick_up.py index 3213f9eb2..f05eeaa97 100644 --- a/embodichain/lab/sim/atomic_skills/primitives/pick_up.py +++ b/embodichain/lab/sim/atomic_skills/primitives/pick_up.py @@ -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) @@ -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: @@ -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 @@ -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(): diff --git a/embodichain/lab/sim/atomic_skills/primitives/place.py b/embodichain/lab/sim/atomic_skills/primitives/place.py index d65249429..b44ac74df 100644 --- a/embodichain/lab/sim/atomic_skills/primitives/place.py +++ b/embodichain/lab/sim/atomic_skills/primitives/place.py @@ -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.") diff --git a/embodichain/lab/task_program/compiler/lowering.py b/embodichain/lab/task_program/compiler/lowering.py index c2d01e751..cbfd3ebcf 100644 --- a/embodichain/lab/task_program/compiler/lowering.py +++ b/embodichain/lab/task_program/compiler/lowering.py @@ -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 @@ -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)) @@ -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) @@ -1966,6 +1994,7 @@ def _downstream_targets( preserve_current_object_orientation=( place_options.preserve_current_object_orientation ), + approach_clearance=place_options.approach_clearance, ) ) break @@ -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), diff --git a/embodichain/lab/task_program/integrations/_configured_services.py b/embodichain/lab/task_program/integrations/_configured_services.py index 7178646a5..b6e92f8f1 100644 --- a/embodichain/lab/task_program/integrations/_configured_services.py +++ b/embodichain/lab/task_program/integrations/_configured_services.py @@ -1009,6 +1009,7 @@ def pick_lookahead_targets( preserve_current_object_orientation=( options.preserve_current_object_orientation ), + approach_clearance=options.approach_clearance, ), ) diff --git a/embodichain/lab/task_program/integrations/configured.py b/embodichain/lab/task_program/integrations/configured.py index f65c2691b..725ba3fa3 100644 --- a/embodichain/lab/task_program/integrations/configured.py +++ b/embodichain/lab/task_program/integrations/configured.py @@ -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", @@ -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 () @@ -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"], diff --git a/tests/gym/envs/task_program/test_configured_integration.py b/tests/gym/envs/task_program/test_configured_integration.py index dc237d6fb..8443f24e7 100644 --- a/tests/gym/envs/task_program/test_configured_integration.py +++ b/tests/gym/envs/task_program/test_configured_integration.py @@ -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: diff --git a/tests/lab/task_program/test_semantic_compiler.py b/tests/lab/task_program/test_semantic_compiler.py index 3f36af3c1..87d00ce97 100644 --- a/tests/lab/task_program/test_semantic_compiler.py +++ b/tests/lab/task_program/test_semantic_compiler.py @@ -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( diff --git a/tests/sim/atomic_skills/test_actions.py b/tests/sim/atomic_skills/test_actions.py index c2d3391ad..957437a17 100644 --- a/tests/sim/atomic_skills/test_actions.py +++ b/tests/sim/atomic_skills/test_actions.py @@ -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)