diff --git a/agent_context/topics/atomic-actions/execution.md b/agent_context/topics/atomic-actions/execution.md index 1558b9efb..0b1b37058 100644 --- a/agent_context/topics/atomic-actions/execution.md +++ b/agent_context/topics/atomic-actions/execution.md @@ -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; diff --git a/embodichain/lab/sim/atomic_skills/primitives/pick_up.py b/embodichain/lab/sim/atomic_skills/primitives/pick_up.py index 3213f9eb2..aeeb93e4b 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.""" + 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) @@ -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 ): @@ -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, @@ -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, diff --git a/embodichain/lab/task_program/integrations/configured.py b/embodichain/lab/task_program/integrations/configured.py index f65c2691b..c8baa6d97 100644 --- a/embodichain/lab/task_program/integrations/configured.py +++ b/embodichain/lab/task_program/integrations/configured.py @@ -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", @@ -1160,6 +1161,7 @@ def _decode_skill_options(value: object, *, path: str) -> SkillOptions: *( ( "approach_along_grasp_axis", + "approach_tilts", "initial_lift_height", "cartesian_lift", ) @@ -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"], diff --git a/tests/sim/atomic_skills/test_actions.py b/tests/sim/atomic_skills/test_actions.py index c2d3391ad..00debea40 100644 --- a/tests/sim/atomic_skills/test_actions.py +++ b/tests/sim/atomic_skills/test_actions.py @@ -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()