diff --git a/agent_context/topics/atomic-actions/atomic-actions.md b/agent_context/topics/atomic-actions/atomic-actions.md index 41f35492c..8201877ea 100644 --- a/agent_context/topics/atomic-actions/atomic-actions.md +++ b/agent_context/topics/atomic-actions/atomic-actions.md @@ -46,6 +46,19 @@ ActionInvocation semantic program Planning never steps simulation and never treats command completion as proof of a physical effect. +`MoveHeldObject` accepts ordered object-pose waypoints without rebasing the +verified grasp. Its goal optionally frees only the final world yaw; default +goals remain exact. Heading alternatives are checked as complete motion paths, +with accepted rows retained independently and selected offsets in diagnostics. + +`SlideGoal.joint_target` optionally supplies a public `SlideJointTarget` with an +absolute prismatic coordinate and a calibrated axis sign. Slide resolves the +remaining distance from same-timestamp joint/robot observations per planning row; +already-satisfied rows hold, invalid observations fail, and mixed active directions +are rejected. Omitting the target preserves fixed-distance `SlideOptions` and the +original Slide tutorial. Slide remains open-loop; this extension does not certify +contact or physical goal satisfaction. + ## Package map | Concern | Source of truth | diff --git a/docs/source/api_reference/public_api.rst b/docs/source/api_reference/public_api.rst index aa4282628..21099d70a 100644 --- a/docs/source/api_reference/public_api.rst +++ b/docs/source/api_reference/public_api.rst @@ -690,6 +690,7 @@ embodichain.lab.sim.atomic_actions.control GRASP_COMMAND JointPositionCommand OPEN_COMMAND + PARK_COMMAND embodichain.lab.sim.atomic_actions.core --------------------------------------- diff --git a/docs/source/overview/sim/atomic_actions/builtin_actions.md b/docs/source/overview/sim/atomic_actions/builtin_actions.md index ea8e1a9cb..75c7848ec 100644 --- a/docs/source/overview/sim/atomic_actions/builtin_actions.md +++ b/docs/source/overview/sim/atomic_actions/builtin_actions.md @@ -231,9 +231,10 @@ control-part adapter resolves current joint-backed endpoints through `Robot.control_parts`; custom adapters may instead return mobile, whole-body, or other runtime targets. -`MoveJoints` is intentionally `agent_visible=False`: it is useful for home, -recovery, calibration, and scripted postures, but is not exposed to an Action -Agent by default. +`MoveJoints` is also the canonical implementation for home, recovery, +calibration, and other embodiment-named postures. Semantic callers should +constrain those uses through a registered call and keep the named target in the +robot profile. ## Shared goal and configuration rules @@ -253,7 +254,11 @@ Explicit pose tensors use `(4, 4)` or `(B, 4, 4)`. Waypoint-capable fields in `EndEffectorPoseGoal` and `PlaceGoal` also accept `(B, N, 4, 4)`. `SceneEntityPose` resolves to the latest `(B, 4, 4)` pose from each `SceneSnapshot`, checks optional perception confidence, and registers that -entity as a recovery dependency. +entity as a recovery dependency. `world_displacement` keeps a translation in +the world frame after local composition. `world_orientation` replaces the +tracked entity's rotation before applying `relative_pose`, allowing a target to +track a moving reference position while retaining a grounded world-frame +orientation. | Skill / field | `SceneEntityPose` accepted | Automatic scene-motion replan | |---|---:|---:| @@ -379,7 +384,7 @@ than an EEF pose. | Motion | joint planning/interpolation from observed qpos; supports joint waypoints | | Completion | `JOINT_GOAL_REACHED` | | Effect | none | -| Agent visibility | hidden by default (`agent_visible=False`) | +| Agent visibility | visible | `target` accepts an explicit qpos tensor with shape `(control_dof,)`, `(B, control_dof)`, or `(B, N, control_dof)`, or a non-empty string resolved @@ -1020,6 +1025,7 @@ Both bound grasp endpoints must provide `open` and `grasp`. Important - `pre_grasp_distance` and `lift_height`; - `object_motion_keyframes`, `hand_interp_steps`, and `hold_steps`; +- `release`, `release_steps`, `retreat_distance`, and `retreat_steps`; - `approach_direction`, `left_to_right_arm_direction`, and `middle_empty_ratio` for affordance-based left/right grasp sampling. @@ -1112,7 +1118,9 @@ through middle transfer, and from the receiving grasp through final lowering, EEF waypoint rotations remain fixed; only translations change. The final object translation comes from `HandOverGoal.target_pose`, while its execution orientation stays consistent with the handover grasp. `HandOverOptions` owns -only the approach/lift distances and gripper interpolation count. The first +the approach/lift distances, gripper interpolation, hold, and retreat phases. +With `release_at_target=False`, the source hand opens and retreats while the +destination remains the verified holder for a later placement action. The first placement waypoint changes only horizontal coordinates and preserves the handover height exactly; the second waypoint lowers to the final target. diff --git a/docs/source/overview/sim/atomic_actions/index.md b/docs/source/overview/sim/atomic_actions/index.md index 434700baf..f71a23013 100644 --- a/docs/source/overview/sim/atomic_actions/index.md +++ b/docs/source/overview/sim/atomic_actions/index.md @@ -362,7 +362,7 @@ instances to the engine's planning services: ```python engine = AtomicActionEngine(motion_generator, control_profiles=profiles) -# All eleven built-ins are immediately usable by stable skill ID. +# All twelve built-ins are immediately usable by stable skill ID. assert "move_end_effector" in engine.actions assert "pick_up" in engine.actions ``` diff --git a/embodichain/lab/sim/atomic_actions/__init__.py b/embodichain/lab/sim/atomic_actions/__init__.py index a92b31d86..45dea3326 100644 --- a/embodichain/lab/sim/atomic_actions/__init__.py +++ b/embodichain/lab/sim/atomic_actions/__init__.py @@ -38,8 +38,8 @@ from .affordance import ( Affordance, AntipodalAffordance, - AssembleAffordance, AxisAlignAffordance, + AssembleAffordance, InteractionPoints, OpenDoorAffordance, PressAffordance, @@ -64,6 +64,7 @@ GRASP_COMMAND, JointPositionCommand, OPEN_COMMAND, + PARK_COMMAND, ) from .core import AtomicAction, ObjectSemantics, SkillDescriptor from .effects import StateDelta @@ -214,6 +215,7 @@ PushObjectToolCalibration, Slide, SlideGoal, + SlideJointTarget, SlideOptions, Twist, TwistGoal, @@ -274,12 +276,12 @@ "ArticulationAffordanceGeometry", "ArticulationGeometryProvider", "ArticulationJointGeometry", + "AxisAlignAffordance", "create_rigidized_articulation_antipodal_affordance", "ArticulationJointState", "AssembleAffordance", "AssembleGoal", "AxisAlign", - "AxisAlignAffordance", "AxisAlignGoal", "AxisAlignOptions", "AtomicAction", @@ -377,6 +379,7 @@ "OPEN_COMMAND", "ObservationProvider", "ObservedArticulationJointState", + "PARK_COMMAND", "PickUp", "PickUpOptions", "Place", @@ -402,6 +405,7 @@ "SlideAffordance", "Slide", "SlideGoal", + "SlideJointTarget", "SlideOptions", "Twist", "TwistGoal", diff --git a/embodichain/lab/sim/atomic_actions/control.py b/embodichain/lab/sim/atomic_actions/control.py index 3af145d9c..d07b21479 100644 --- a/embodichain/lab/sim/atomic_actions/control.py +++ b/embodichain/lab/sim/atomic_actions/control.py @@ -31,6 +31,9 @@ GRASP_COMMAND = "grasp" """Conventional semantic command for an object-holding end effector.""" +PARK_COMMAND = "park" +"""Conventional semantic command for an embodiment-defined parked posture.""" + class ControlCommand(ABC): """Immutable-by-ownership command associated with one control part. @@ -282,4 +285,5 @@ def as_flat_mapping( "GRASP_COMMAND", "JointPositionCommand", "OPEN_COMMAND", + "PARK_COMMAND", ] diff --git a/embodichain/lab/sim/atomic_actions/core.py b/embodichain/lab/sim/atomic_actions/core.py index cb4fd5296..c2c8fa00c 100644 --- a/embodichain/lab/sim/atomic_actions/core.py +++ b/embodichain/lab/sim/atomic_actions/core.py @@ -50,7 +50,7 @@ normalize_success_mask, ) from .policies import DynamicCollisionMode -from .requirements import SkillBindingContract +from .requirements import GRASP_CAPABILITY, SkillBindingContract from .runtime_commands import ( EndpointCommand, JointPositionPayload, @@ -745,6 +745,12 @@ def _tracking_sequence( for command in frame.commands: endpoints = endpoints_by_destination[command.destination_key] for endpoint in endpoints: + # Contact intentionally prevents grasping endpoints from + # reaching their unobstructed close setpoint. Attachment + # and release are accepted by typed effect evidence; joint + # tracking remains authoritative for motion endpoints. + if GRASP_CAPABILITY in endpoint.capabilities: + continue for channel_id in metrics_by_channel: channel = endpoint.tracking_channels.get(channel_id) if channel is None: diff --git a/embodichain/lab/sim/atomic_actions/execution.py b/embodichain/lab/sim/atomic_actions/execution.py index 920003493..9f967c8fc 100644 --- a/embodichain/lab/sim/atomic_actions/execution.py +++ b/embodichain/lab/sim/atomic_actions/execution.py @@ -1256,6 +1256,13 @@ def _install_plan( def _validate_phase_effect_gates(self, plan: ActionPlan) -> None: """Bind invocation-owned gates to non-initial named plan segments.""" + if not plan.plan_success.any(): + # A fully failed plan owns no executable phase boundary. Preserve + # its typed PlannerDiagnostics so ordinary retry/exhaustion logic + # can handle the failure; validating gate segment names against an + # empty failed-plan trajectory would replace that cause with a + # misleading preparation error. + return request = self._requests[self._invocation_index] for requirement in request.phase_effect_gates: if type(requirement) is not PhaseEffectGateRequirement: diff --git a/embodichain/lab/sim/atomic_actions/goals.py b/embodichain/lab/sim/atomic_actions/goals.py index e3fb94fc8..87f8e46d4 100644 --- a/embodichain/lab/sim/atomic_actions/goals.py +++ b/embodichain/lab/sim/atomic_actions/goals.py @@ -47,6 +47,12 @@ class SceneEntityPose: minimum_confidence: float = 0.0 """Minimum accepted perception confidence.""" + world_displacement: torch.Tensor | None = None + """Optional world-frame translation applied after ``relative_pose``.""" + + world_orientation: torch.Tensor | None = None + """Optional world-frame orientation applied before ``relative_pose``.""" + def __post_init__(self) -> None: if not isinstance(self.entity_id, str) or not self.entity_id.strip(): raise ValueError("entity_id must be a non-empty string.") @@ -57,6 +63,32 @@ def __post_init__(self) -> None: allow_waypoints=False, ) object.__setattr__(self, "relative_pose", self.relative_pose.clone()) + if self.world_displacement is not None: + displacement = self.world_displacement + if not isinstance(displacement, torch.Tensor): + raise TypeError("world_displacement must be a torch.Tensor or None.") + if displacement.dim() not in (1, 2) or displacement.shape[-1] != 3: + raise ValueError( + "world_displacement must have shape (3,) or (num_envs, 3)." + ) + if displacement.dim() == 2 and displacement.shape[0] == 0: + raise ValueError("world_displacement batches must not be empty.") + if not torch.isfinite(displacement).all(): + raise ValueError("world_displacement must contain finite values.") + object.__setattr__(self, "world_displacement", displacement.clone()) + if self.world_orientation is not None: + orientation = self.world_orientation + if not isinstance(orientation, torch.Tensor): + raise TypeError("world_orientation must be a torch.Tensor or None.") + if orientation.dim() not in (2, 3) or orientation.shape[-2:] != (3, 3): + raise ValueError( + "world_orientation must have shape (3, 3) or " "(num_envs, 3, 3)." + ) + if orientation.dim() == 3 and orientation.shape[0] == 0: + raise ValueError("world_orientation batches must not be empty.") + if not torch.isfinite(orientation).all(): + raise ValueError("world_orientation must contain finite values.") + object.__setattr__(self, "world_orientation", orientation.clone()) if not 0.0 <= self.minimum_confidence <= 1.0: raise ValueError("minimum_confidence must be in [0, 1].") @@ -70,6 +102,8 @@ def snapshot(self) -> SceneEntityPose: self.entity_id, relative_pose=self.relative_pose, minimum_confidence=self.minimum_confidence, + world_displacement=self.world_displacement, + world_orientation=self.world_orientation, ) @@ -154,14 +188,34 @@ def resolve_pose_goal( raise ValueError( f"Scene entity {value.entity_id!r} pose must match planning batch size." ) + if value.world_orientation is not None: + orientation = value.world_orientation.to(device=pose.device, dtype=pose.dtype) + if orientation.shape == (3, 3): + orientation = orientation.unsqueeze(0).expand(context.batch_size, -1, -1) + elif orientation.shape != (context.batch_size, 3, 3): + raise ValueError( + f"{name}.world_orientation must match planning batch size." + ) + pose = pose.clone() + pose[:, :3, :3] = orientation if value.relative_pose is None: - return pose.clone() - relative = value.relative_pose.to(device=pose.device, dtype=pose.dtype) - if relative.shape == (4, 4): - relative = relative.unsqueeze(0).expand(context.batch_size, -1, -1) - elif relative.shape != (context.batch_size, 4, 4): - raise ValueError(f"{name}.relative_pose must match planning batch size.") - return torch.bmm(pose, relative) + resolved = pose.clone() + else: + relative = value.relative_pose.to(device=pose.device, dtype=pose.dtype) + if relative.shape == (4, 4): + relative = relative.unsqueeze(0).expand(context.batch_size, -1, -1) + elif relative.shape != (context.batch_size, 4, 4): + raise ValueError(f"{name}.relative_pose must match planning batch size.") + resolved = torch.bmm(pose, relative) + if value.world_displacement is None: + return resolved + displacement = value.world_displacement.to(device=pose.device, dtype=pose.dtype) + if displacement.shape == (3,): + displacement = displacement.unsqueeze(0).expand(context.batch_size, -1) + elif displacement.shape != (context.batch_size, 3): + raise ValueError(f"{name}.world_displacement must match planning batch size.") + resolved[:, :3, 3] += displacement + return resolved def _resolve_object_pose( diff --git a/embodichain/lab/sim/atomic_actions/primitives/__init__.py b/embodichain/lab/sim/atomic_actions/primitives/__init__.py index 01de334ee..4f8011e3e 100644 --- a/embodichain/lab/sim/atomic_actions/primitives/__init__.py +++ b/embodichain/lab/sim/atomic_actions/primitives/__init__.py @@ -56,6 +56,7 @@ from .slide import ( Slide, SlideGoal, + SlideJointTarget, SlideOptions, ) from .twist import Twist, TwistGoal, TwistOptions @@ -124,6 +125,7 @@ "PushObjectToolCalibration", "Slide", "SlideGoal", + "SlideJointTarget", "SlideOptions", "Twist", "TwistGoal", diff --git a/embodichain/lab/sim/atomic_actions/primitives/axis_align.py b/embodichain/lab/sim/atomic_actions/primitives/axis_align.py index 638264b31..123f6749c 100644 --- a/embodichain/lab/sim/atomic_actions/primitives/axis_align.py +++ b/embodichain/lab/sim/atomic_actions/primitives/axis_align.py @@ -424,6 +424,19 @@ def _plan( ), ), segment_lengths=segment_lengths, + # Contact during close/manipulate intentionally moves the aligned + # semantic object. Independent scene-referenced grasp targets + # remain monitored for the full action. + scene_dependency_monitor_until={ + entity_id: max( + 1, + math.ceil( + segment_lengths["approach"] * options.grasp_commit_fraction + ), + ) + for entity_id in self._scene_dependencies(request) + if entity_id == request.goal.semantics.entity_id + }, ) def _resolve_grasp_pose( diff --git a/embodichain/lab/sim/atomic_actions/primitives/coordinated_pickment.py b/embodichain/lab/sim/atomic_actions/primitives/coordinated_pickment.py index 459b1555b..4ba1c433f 100644 --- a/embodichain/lab/sim/atomic_actions/primitives/coordinated_pickment.py +++ b/embodichain/lab/sim/atomic_actions/primitives/coordinated_pickment.py @@ -19,6 +19,7 @@ from __future__ import annotations from dataclasses import dataclass +import math from typing import ClassVar import torch @@ -53,6 +54,7 @@ normalize_success_mask, ) from embodichain.lab.sim.atomic_actions.requirements import ( + BATCH_INVERSE_KINEMATICS_CAPABILITY, DisjointResourceSlots, INVERSE_KINEMATICS_CAPABILITY, SkillBindingContract, @@ -133,6 +135,18 @@ class CoordinatedPickmentOptions(ActionOptions): hold_steps: int = 4 """Number of waypoints to hold the final object target pose.""" + release: bool = False + """Whether both hands open after reaching the shared object target pose.""" + + release_steps: int = 10 + """Number of waypoints used for the simultaneous hand-open segment.""" + + retreat_distance: float = 0.08 + """World-Z retreat distance after a coordinated release.""" + + retreat_steps: int = 12 + """Number of waypoints used for the simultaneous post-release retreat.""" + approach_direction: torch.Tensor = torch.tensor( [0.0, 0.0, -1.0], dtype=torch.float32 ) @@ -147,18 +161,34 @@ class CoordinatedPickmentOptions(ActionOptions): middle_empty_ratio: float = 0.4 """Fraction of the object's left-to-right extent left grasp-free in the middle - so the two grippers pinch opposite ends. Must be in ``[0, 1]``.""" + so the two grippers pinch opposite ends. Must be in ``[0, 1)``.""" + + grasp_seed: int = 17_393 + """Deterministic seed isolated around coordinated grasp sampling.""" def __post_init__(self) -> None: if self.object_motion_keyframes < 2: raise ValueError("object_motion_keyframes must be at least 2.") - if self.pre_grasp_distance < 0.0: - raise ValueError("pre_grasp_distance must be non-negative.") - if self.lift_height < 0.0: - raise ValueError("lift_height must be non-negative.") - for name in ("hand_interp_steps", "hold_steps"): - if getattr(self, name) < 0: - raise ValueError(f"{name} must be non-negative.") + for name in ("pre_grasp_distance", "lift_height", "retreat_distance"): + value = getattr(self, name) + if ( + isinstance(value, bool) + or not isinstance(value, (int, float)) + or not math.isfinite(value) + or value < 0.0 + ): + raise ValueError(f"{name} must be finite and non-negative.") + if not isinstance(self.release, bool): + raise TypeError("release must be a bool.") + for name in ( + "hand_interp_steps", + "hold_steps", + "release_steps", + "retreat_steps", + ): + value = getattr(self, name) + if type(value) is not int or value < 0: + raise ValueError(f"{name} must be a non-negative integer.") for name in ("approach_direction", "left_to_right_arm_direction"): value = getattr(self, name) if value.shape != (3,): @@ -168,8 +198,15 @@ def __post_init__(self) -> None: if torch.linalg.vector_norm(value) <= 1.0e-6: raise ValueError(f"{name} must be non-zero.") object.__setattr__(self, name, value.clone()) - if not 0.0 <= self.middle_empty_ratio <= 1.0: - raise ValueError("middle_empty_ratio must be in [0, 1].") + if ( + isinstance(self.middle_empty_ratio, bool) + or not isinstance(self.middle_empty_ratio, (int, float)) + or not math.isfinite(self.middle_empty_ratio) + or not 0.0 <= self.middle_empty_ratio < 1.0 + ): + raise ValueError("middle_empty_ratio must be finite and in [0, 1).") + if type(self.grasp_seed) is not int or self.grasp_seed < 0: + raise ValueError("grasp_seed must be a non-negative integer.") @dataclass(frozen=True, slots=True, eq=False) @@ -356,11 +393,17 @@ class CoordinatedPickment( skill_id: ClassVar[str] = "coordinated_pickment" GoalType: ClassVar[type] = CoordinatedPickGoal OptionsType: ClassVar[type] = CoordinatedPickmentOptions + _MAX_REACHABILITY_CANDIDATES: ClassVar[int] = 32 binding_contract: ClassVar[SkillBindingContract] = SkillBindingContract( slots=tuple( make_manipulation_slot( role, - motion_capabilities=frozenset({INVERSE_KINEMATICS_CAPABILITY}), + motion_capabilities=frozenset( + { + INVERSE_KINEMATICS_CAPABILITY, + BATCH_INVERSE_KINEMATICS_CAPABILITY, + } + ), grasp_commands={ OPEN_COMMAND: JointPositionCommand, GRASP_COMMAND: JointPositionCommand, @@ -498,6 +541,8 @@ def _resolve_target( context: PlanningContext, options: CoordinatedPickmentOptions, resources: _CoordinatedPickResources, + left_start_qpos: torch.Tensor, + right_start_qpos: torch.Tensor, ) -> tuple[ torch.Tensor, torch.Tensor, @@ -521,9 +566,14 @@ def _resolve_target( self._resolve_dual_arm_grasp_poses( target.semantics, object_initial_pose, + object_target_pose, options, resources.left_hand.target_id, resources.right_hand.target_id, + left_start_qpos, + right_start_qpos, + resources.left_arm.control_part, + resources.right_arm.control_part, ) ) left_object_to_eef = torch.bmm(pose_inv(object_initial_pose), left_grasp_xpos) @@ -557,19 +607,30 @@ def _resolve_dual_arm_grasp_poses( self, semantics: ObjectSemantics, object_poses: torch.Tensor, + object_target_poses: torch.Tensor, options: CoordinatedPickmentOptions, left_grasp_target_id: str, right_grasp_target_id: str, + left_start_qpos: torch.Tensor, + right_start_qpos: torch.Tensor, + left_control_part: str, + right_control_part: str, ) -> tuple[torch.Tensor, torch.Tensor, torch.Tensor]: """Sample left/right grasp poses from the target antipodal affordance. Args: semantics: Object semantics carrying an :class:`AntipodalAffordance`. object_poses: Object poses with shape ``(num_envs, 4, 4)``. + object_target_poses: Requested terminal object poses with shape + ``(num_envs, 4, 4)``. options: Coordinated pickment options carrying the dual-arm and approach directions used by the grasp-pose generator. left_grasp_target_id: Left grasp endpoint target ID. right_grasp_target_id: Right grasp endpoint target ID. + left_start_qpos: Current left-arm joint positions. + right_start_qpos: Current right-arm joint positions. + left_control_part: Bound left-arm control-part name. + right_control_part: Bound right-arm control-part name. Returns: ``(left_grasp_xpos, right_grasp_xpos, success_mask)``. The grasp poses @@ -583,11 +644,6 @@ def _resolve_dual_arm_grasp_poses( "dual-arm grasps." ) semantics.affordance.require_whole_object_mesh() - num_envs = object_poses.shape[0] - identity = torch.eye(4, dtype=torch.float32, device=self.device) - left_grasp_xpos = identity.unsqueeze(0).repeat(num_envs, 1, 1) - right_grasp_xpos = identity.unsqueeze(0).repeat(num_envs, 1, 1) - success_mask = torch.zeros(num_envs, dtype=torch.bool, device=self.device) approach_direction = options.approach_direction.to( device=self.device, dtype=torch.float32 ) @@ -603,72 +659,546 @@ def _resolve_dual_arm_grasp_poses( right_generator = self.planning_services.grasp_pose_generator( right_grasp_target_id ) - if left_generator is not right_generator: - raise ValueError( - "CoordinatedPickment currently requires the left and right " - "grasp endpoints to share one grasp-pose generator instance." - ) - if not isinstance(left_generator, ParallelJawGraspPoseGenerator): + if not isinstance( + left_generator, ParallelJawGraspPoseGenerator + ) or not isinstance(right_generator, ParallelJawGraspPoseGenerator): raise TypeError( - "CoordinatedPickment requires a " "ParallelJawGraspPoseGenerator." + "CoordinatedPickment requires ParallelJawGraspPoseGenerator " + "services for both grasp endpoints." ) - dual_results = left_generator.get_dual_arm_valid_grasp_poses( - mesh_vertices=semantics.affordance.mesh_vertices, - mesh_triangles=semantics.affordance.mesh_triangles, - obj_poses=object_poses, - left_to_right_arm_direction=left_to_right_arm_direction, - approach_direction=approach_direction, - middle_empty_ratio=options.middle_empty_ratio, - ) - for env_idx, result in enumerate(dual_results): - if result is None: - logger.log_warning( - f"Failed to sample dual-arm grasps for environment {env_idx}." + if left_generator.gripper_model != right_generator.gripper_model: + raise ValueError( + "CoordinatedPickment requires matching left and right parallel-jaw " + "gripper geometry." + ) + partition_ratios = self._candidate_middle_empty_ratios( + semantics.affordance, + object_poses, + left_to_right_arm_direction, + base_ratio=options.middle_empty_ratio, + ) + approach_directions = self._candidate_approach_directions( + semantics.affordance, + object_poses, + left_to_right_arm_direction, + requested=approach_direction, + ) + identity = torch.eye(4, dtype=torch.float32, device=self.device).repeat( + self.num_envs, + 1, + 1, + ) + left_grasp_xpos = identity.clone() + right_grasp_xpos = identity.clone() + success_mask = torch.zeros( + self.num_envs, + dtype=torch.bool, + device=self.device, + ) + sampling_device = torch.device(self.device) + cuda_devices = ( + [ + ( + torch.cuda.current_device() + if sampling_device.index is None + else sampling_device.index ) - continue - left_grasp = self._select_best_grasp(result["left"]) - right_grasp = self._select_best_grasp(result["right"]) - if left_grasp is None or right_grasp is None: - logger.log_warning( - f"No valid left/right grasp for environment {env_idx}." + ] + if sampling_device.type == "cuda" + else [] + ) + selected_candidate: tuple[int, int] | None = None + for approach_index, candidate_approach in enumerate(approach_directions): + for partition_index, partition_ratio in enumerate(partition_ratios): + # GraspKit perturbs approach directions while building candidates. + # Isolate that randomness so retries and alternative partitions do + # not consume or depend on application-global RNG state. Reusing + # one seed also keeps the sampled surface realization fixed while + # geometry policies are the only variables under evaluation. + with torch.random.fork_rng(devices=cuda_devices): + torch.manual_seed(options.grasp_seed) + if cuda_devices: + torch.cuda.manual_seed_all(options.grasp_seed) + dual_results = left_generator.get_dual_arm_valid_grasp_poses( + mesh_vertices=semantics.affordance.mesh_vertices, + mesh_triangles=semantics.affordance.mesh_triangles, + obj_poses=object_poses, + left_to_right_arm_direction=left_to_right_arm_direction, + approach_direction=candidate_approach, + middle_empty_ratio=partition_ratio, + ) + candidate_left, left_success = self._select_reachable_arm_grasp( + dual_results, + role="left", + object_poses=object_poses, + object_target_poses=object_target_poses, + start_qpos=left_start_qpos, + control_part=left_control_part, + options=options, + log_failure=False, ) - continue - left_grasp_xpos[env_idx] = left_grasp.to( - device=self.device, dtype=torch.float32 + candidate_right, right_success = self._select_reachable_arm_grasp( + dual_results, + role="right", + object_poses=object_poses, + object_target_poses=object_target_poses, + start_qpos=right_start_qpos, + control_part=right_control_part, + options=options, + log_failure=False, + ) + selected = ~success_mask & left_success & right_success + left_grasp_xpos = torch.where( + selected[:, None, None], + candidate_left, + left_grasp_xpos, + ) + right_grasp_xpos = torch.where( + selected[:, None, None], + candidate_right, + right_grasp_xpos, + ) + success_mask |= selected + if bool(selected.any().item()) and selected_candidate is None: + selected_candidate = (approach_index, partition_index) + if success_mask.all(): + break + if success_mask.all(): + break + if not success_mask.all(): + failed = torch.nonzero(~success_mask, as_tuple=False).flatten().tolist() + logger.log_warning( + "No jointly reachable coordinated grasp for environment(s) " + f"{failed}; tried {len(approach_directions)} approach directions " + f"and middle-empty ratios {list(partition_ratios)}." ) - right_grasp_xpos[env_idx] = right_grasp.to( - device=self.device, dtype=torch.float32 + elif selected_candidate is not None: + approach_index, partition_index = selected_candidate + logger.log_info( + "Selected coordinated grasp candidate with approach direction " + f"{approach_directions[approach_index].detach().cpu().tolist()} " + f"and middle-empty ratio {partition_ratios[partition_index]}." ) - success_mask[env_idx] = True return left_grasp_xpos, right_grasp_xpos, success_mask @staticmethod - def _select_best_grasp(arm_result: dict) -> torch.Tensor | None: - """Return the lowest-cost grasp pose from one arm's sampler result. + def _candidate_middle_empty_ratios( + affordance: AntipodalAffordance, + object_poses: torch.Tensor, + left_to_right_arm_direction: torch.Tensor, + *, + base_ratio: float, + ) -> tuple[float, ...]: + """Rank deterministic dual-grasp partitions from live object geometry. + + The live-axis candidate respects the current object orientation. An + axis-aligned geometry candidate remains in the bounded search because + a rotated container can otherwise make the projected span too narrow + for both grippers even though its physical end regions remain usable. + """ + vertices = affordance.mesh_vertices + if ( + not isinstance(vertices, torch.Tensor) + or vertices.dim() != 2 + or vertices.shape[0] < 3 + or vertices.shape[1] != 3 + or not bool(torch.isfinite(vertices).all().item()) + ): + return (float(base_ratio),) + local_vertices = vertices.to( + device=object_poses.device, + dtype=object_poses.dtype, + ) + centered = local_vertices - local_vertices.mean(dim=0, keepdim=True) + covariance = ( + centered.transpose(0, 1) @ centered / float(local_vertices.shape[0]) + ) + eigenvalues, eigenvectors = torch.linalg.eigh(covariance) + second = float(eigenvalues[-2].clamp_min(1.0e-12).item()) + longest = float(eigenvalues[-1].clamp_min(1.0e-12).item()) + elongation_ratio = math.sqrt(longest / second) + elongation_confidence = min( + 1.0, + max(0.0, (elongation_ratio - 1.0) / 1.5), + ) + world_axes = torch.matmul(object_poses[:, :3, :3], eigenvectors) + principal_world = world_axes[:, :, -1] + principal_world = principal_world / torch.linalg.vector_norm( + principal_world, + dim=1, + keepdim=True, + ).clamp_min(1.0e-6) + arm_alignment = torch.abs( + torch.sum( + principal_world * left_to_right_arm_direction[None], + dim=1, + ) + ) + geometric_ratio = 0.25 + 0.45 * float(arm_alignment.mean().item()) + preferred_ratio = (1.0 - elongation_confidence) * float( + base_ratio + ) + elongation_confidence * geometric_ratio + axis_aligned_ratio = (1.0 - elongation_confidence) * float( + base_ratio + ) + elongation_confidence * 0.70 + ratios: list[float] = [] + for raw_ratio in ( + preferred_ratio, + axis_aligned_ratio, + float(base_ratio), + preferred_ratio - 0.15, + preferred_ratio + 0.15, + ): + ratio = min(0.90, max(0.05, raw_ratio)) + if not any(abs(ratio - existing) <= 1.0e-6 for existing in ratios): + ratios.append(ratio) + return tuple(ratios) - Args: - arm_result: One ``"left"``/``"right"`` result from the installed - parallel-jaw grasp-pose generator. + @staticmethod + def _candidate_approach_directions( + affordance: AntipodalAffordance, + object_poses: torch.Tensor, + left_to_right_arm_direction: torch.Tensor, + *, + requested: torch.Tensor, + ) -> tuple[torch.Tensor, ...]: + """Build a bounded world-frame approach search for coordinated grasps.""" + candidates: list[torch.Tensor] = [] + + def add(direction: torch.Tensor) -> None: + value = direction.to(device=object_poses.device, dtype=object_poses.dtype) + norm = torch.linalg.vector_norm(value) + if not bool(torch.isfinite(value).all().item()) or float(norm) <= 1.0e-6: + return + value = value / norm + if any( + float(torch.dot(value, existing).item()) >= 1.0 - 1.0e-5 + for existing in candidates + ): + return + candidates.append(value) + + down = requested.new_tensor([0.0, 0.0, -1.0]) + add(requested) + add(down) + horizontal_arm = left_to_right_arm_direction.clone() + horizontal_arm[2] = 0.0 + horizontal_norm = torch.linalg.vector_norm(horizontal_arm) + if float(horizontal_norm) > 1.0e-6: + horizontal_arm = horizontal_arm / horizontal_norm + robot_forward = torch.stack( + (-horizontal_arm[1], horizontal_arm[0], horizontal_arm.new_tensor(0.0)) + ) + add(robot_forward + down) + add(-robot_forward + down) + add(robot_forward) + add(-robot_forward) - Returns: - The selected ``(4, 4)`` grasp pose, or ``None`` when the sampler - reports no valid grasp for this arm. + vertices = affordance.mesh_vertices + if ( + isinstance(vertices, torch.Tensor) + and vertices.dim() == 2 + and vertices.shape[0] >= 3 + and vertices.shape[1] == 3 + and bool(torch.isfinite(vertices).all().item()) + ): + local_vertices = vertices.to( + device=object_poses.device, + dtype=object_poses.dtype, + ) + centered = local_vertices - local_vertices.mean(dim=0, keepdim=True) + covariance = ( + centered.transpose(0, 1) @ centered / float(local_vertices.shape[0]) + ) + _, eigenvectors = torch.linalg.eigh(covariance) + world_axes = torch.matmul(object_poses[:, :3, :3], eigenvectors) + for axis_index in range(3): + axes = world_axes[:, :, axis_index] + if float(torch.mean(torch.abs(axes[:, 2])).item()) > 0.75: + continue + reference = axes[0] + consistency = torch.abs(torch.matmul(axes, reference)) + if bool((consistency < 0.90).any().item()): + continue + add(reference + down) + add(-reference + down) + add(reference) + add(-reference) + break + return tuple(candidates) + + def _select_reachable_arm_grasp( + self, + dual_results: list[dict[str, dict[str, object]] | None], + *, + role: str, + object_poses: torch.Tensor, + object_target_poses: torch.Tensor, + start_qpos: torch.Tensor, + control_part: str, + options: CoordinatedPickmentOptions, + log_failure: bool = True, + ) -> tuple[torch.Tensor, torch.Tensor]: + """Select the lowest-cost candidate with a reachable transport route.""" + live_eef_pose = self.robot.compute_fk( + qpos=start_qpos, + name=control_part, + to_matrix=True, + ) + live_eef_pose = torch.as_tensor( + live_eef_pose, + dtype=torch.float32, + device=self.device, + ) + if live_eef_pose.shape != (self.num_envs, 4, 4): + raise ValueError( + f"Current {control_part} pose must have shape " + f"({self.num_envs}, 4, 4), got {tuple(live_eef_pose.shape)}." + ) + candidates, costs, sampled = self._ranked_arm_grasp_candidates( + dual_results, + role=role, + live_eef_pose=live_eef_pose, + ) + candidate_count = candidates.shape[1] + object_to_eef = torch.matmul( + pose_inv(object_poses)[:, None], + candidates, + ) + pre_grasp = candidates.clone() + pre_grasp[..., :3, 3] -= pre_grasp[..., :3, 2] * options.pre_grasp_distance + lifted_object = translate_pose_world( + object_poses, + torch.tensor( + [0.0, 0.0, options.lift_height], + dtype=object_poses.dtype, + device=self.device, + ), + ) + lifted_eef = torch.matmul(lifted_object[:, None], object_to_eef) + stages: list[tuple[str, torch.Tensor]] = [ + ("pre_grasp", pre_grasp), + ("grasp", candidates), + ("lift", lifted_eef), + ] + + # Screen the transport with the same continuation used by the final + # synchronized plan. Solving the terminal pose directly from the lift + # seed can reject a reachable route when a sparse IK sampler needs the + # intermediate solutions to stay on one joint-space branch. + transport_keyframes = self._interpolate_object_pose( + lifted_object, + object_target_poses, + max(2, options.object_motion_keyframes), + include_orientation=True, + ) + for keyframe_index in range(1, transport_keyframes.shape[1]): + stages.append( + ( + f"transport_{keyframe_index}", + torch.matmul( + transport_keyframes[:, keyframe_index, None], + object_to_eef, + ), + ) + ) + target_eef = stages[-1][1] + if options.release and options.retreat_distance > 0.0: + retreat_eef = target_eef.clone() + retreat_eef[..., 2, 3] += options.retreat_distance + stages.append(("retreat", retreat_eef)) + + seed = start_qpos[:, None, :].expand(-1, candidate_count, -1).clone() + feasible = sampled.clone() + stage_counts: dict[str, list[int]] = {} + for stage_name, stage_poses in stages: + result = self.robot.compute_batch_ik( + pose=stage_poses, + name=control_part, + joint_seed=seed, + ) + if type(result) is not tuple or len(result) != 2: + raise TypeError( + "CoordinatedPickment batch IK must return (success, qpos)." + ) + stage_success = torch.as_tensor( + result[0], + dtype=torch.bool, + device=self.device, + ) + stage_qpos = torch.as_tensor( + result[1], + dtype=torch.float32, + device=self.device, + ) + if stage_success.shape != sampled.shape: + raise ValueError( + f"Batch IK success for {control_part} {stage_name} must have " + f"shape {tuple(sampled.shape)}, got {tuple(stage_success.shape)}." + ) + if stage_qpos.shape != seed.shape: + raise ValueError( + f"Batch IK qpos for {control_part} {stage_name} must have " + f"shape {tuple(seed.shape)}, got {tuple(stage_qpos.shape)}." + ) + feasible &= stage_success + seed = torch.where(feasible[..., None], stage_qpos, seed) + stage_counts[stage_name] = feasible.sum(dim=1).tolist() + + feasible_costs = torch.where( + feasible, + costs, + torch.full_like(costs, torch.inf), + ) + best_cost, best_index = feasible_costs.min(dim=1) + success = torch.isfinite(best_cost) + env_index = torch.arange(candidates.shape[0], device=self.device) + selected = candidates[env_index, best_index] + if log_failure and not success.all(): + failed = torch.nonzero(~success, as_tuple=False).flatten().tolist() + logger.log_warning( + f"CoordinatedPickment {role} route screening failed for " + f"environment(s) {failed}: sampled={sampled.sum(dim=1).tolist()}, " + f"reachable={stage_counts}." + ) + return selected, success + + def _ranked_arm_grasp_candidates( + self, + dual_results: list[dict[str, dict[str, object]] | None], + *, + role: str, + live_eef_pose: torch.Tensor, + ) -> tuple[torch.Tensor, torch.Tensor, torch.Tensor]: + """Pad wrist-canonicalized candidates in ascending total-cost order.""" + if role not in {"left", "right"}: + raise ValueError("Coordinated grasp role must be 'left' or 'right'.") + if live_eef_pose.shape != (len(dual_results), 4, 4): + raise ValueError( + "live_eef_pose must provide one 4x4 pose per dual-grasp result." + ) + ranked: list[tuple[torch.Tensor, torch.Tensor]] = [] + max_candidates = 0 + for row_index, result in enumerate(dual_results): + if result is None: + poses = torch.empty((0, 4, 4), device=self.device) + candidate_costs = torch.empty((0,), device=self.device) + else: + arm_result = result.get(role) + if not isinstance(arm_result, dict) or not arm_result.get( + "is_success", False + ): + poses = torch.empty((0, 4, 4), device=self.device) + candidate_costs = torch.empty((0,), device=self.device) + else: + poses = torch.as_tensor( + arm_result["grasp_poses"], + dtype=torch.float32, + device=self.device, + ) + candidate_costs = torch.as_tensor( + arm_result["total_cost"], + dtype=torch.float32, + device=self.device, + ).reshape(-1) + if poses.shape == (4, 4): + poses = poses.unsqueeze(0) + if poses.dim() != 3 or poses.shape[1:] != (4, 4): + raise ValueError( + f"Coordinated {role} grasp poses must have shape " + f"(N, 4, 4), got {tuple(poses.shape)}." + ) + if poses.shape[0] != candidate_costs.shape[0]: + raise ValueError( + f"Coordinated {role} grasp poses and costs must have " + "equal candidate counts." + ) + finite = torch.isfinite(candidate_costs) + poses, wrist_rotation_cost = self._canonicalize_parallel_jaw_poses( + poses[finite], + live_eef_pose[row_index], + ) + candidate_costs = ( + candidate_costs[finite] + wrist_rotation_cost / math.pi + ) + order = torch.argsort(candidate_costs) + poses = poses[order][: self._MAX_REACHABILITY_CANDIDATES] + candidate_costs = candidate_costs[order][ + : self._MAX_REACHABILITY_CANDIDATES + ] + ranked.append((poses, candidate_costs)) + max_candidates = max(max_candidates, poses.shape[0]) + + padded_count = max(1, max_candidates) + identity = torch.eye(4, dtype=torch.float32, device=self.device) + poses = identity.repeat(len(ranked), padded_count, 1, 1) + costs = torch.full( + (len(ranked), padded_count), + torch.inf, + dtype=torch.float32, + device=self.device, + ) + sampled = torch.zeros_like(costs, dtype=torch.bool) + for env_index, (env_poses, env_costs) in enumerate(ranked): + count = env_poses.shape[0] + if count == 0: + continue + poses[env_index, :count] = env_poses + costs[env_index, :count] = env_costs + sampled[env_index, :count] = True + return poses, costs, sampled + + @staticmethod + def _canonicalize_parallel_jaw_poses( + poses: torch.Tensor, + live_eef_pose: torch.Tensor, + ) -> tuple[torch.Tensor, torch.Tensor]: + """Choose each grasp's local-z half-turn nearest the live wrist. + + A parallel-jaw grasp is physically unchanged by a 180-degree rotation + around its TCP z axis. Canonicalizing that symmetry before IK avoids + rejecting an otherwise reachable top-down grasp solely because the + sampler returned the opposite wrist roll. """ - if not arm_result.get("is_success", False): - return None - grasp_poses = arm_result["grasp_poses"].to(dtype=torch.float32) - costs = arm_result["total_cost"].to(dtype=torch.float32) - if grasp_poses.dim() == 2: - # The sampler returns a single eye(4) placeholder when it finds no - # valid pair; is_success should already cover this, but stay robust. - grasp_poses = grasp_poses.unsqueeze(0) - costs = costs.unsqueeze(0) - if grasp_poses.shape[0] == 0: - return None - best_idx = torch.argmin(costs) - if not torch.isfinite(costs[best_idx]): - return None - return grasp_poses[best_idx] + if poses.dim() != 3 or poses.shape[1:] != (4, 4): + raise ValueError("poses must have shape (N, 4, 4).") + if live_eef_pose.shape != (4, 4): + raise ValueError("live_eef_pose must have shape (4, 4).") + half_turn = torch.eye( + 4, + dtype=poses.dtype, + device=poses.device, + ) + half_turn[0, 0] = -1.0 + half_turn[1, 1] = -1.0 + alternatives = torch.matmul(poses, half_turn) + live_rotation = ( + live_eef_pose[:3, :3] + .unsqueeze(0) + .expand( + poses.shape[0], + -1, + -1, + ) + ) + + def rotation_distance(candidate: torch.Tensor) -> torch.Tensor: + relative = torch.matmul( + live_rotation.transpose(-1, -2), + candidate[:, :3, :3], + ) + cosine = ( + torch.diagonal(relative, dim1=-2, dim2=-1).sum(dim=-1) - 1.0 + ) * 0.5 + return torch.acos(torch.clamp(cosine, -1.0, 1.0)) + + original_cost = rotation_distance(poses) + alternative_cost = rotation_distance(alternatives) + use_alternative = alternative_cost < original_cost + return ( + torch.where(use_alternative[:, None, None], alternatives, poses), + torch.where(use_alternative, alternative_cost, original_cost), + ) def _compute_segment_lengths( self, sample_count: int, options: CoordinatedPickmentOptions @@ -676,14 +1206,16 @@ def _compute_segment_lengths( """Split the invocation sample budget across coordinated-pick segments.""" n_close = max(2, options.hand_interp_steps) n_hold = max(0, options.hold_steps) - n_motion = sample_count - n_close - n_hold + n_release = max(2, options.release_steps) if options.release else 0 + n_retreat = max(2, options.retreat_steps) if options.release else 0 + n_motion = sample_count - n_close - n_hold - n_release - n_retreat n_approach = n_motion // 3 n_lift = n_motion // 3 n_move = n_motion - n_approach - n_lift if min(n_approach, n_lift, n_move) < 2: raise ValueError( "Not enough waypoints for coordinated pickment. Please increase " - "sample_count or decrease hand_interp_steps/hold_steps." + "sample_count or decrease close/hold/release/retreat steps." ) return { "approach": n_approach, @@ -691,6 +1223,8 @@ def _compute_segment_lengths( "lift": n_lift, "move": n_move, "hold": n_hold, + "release": n_release, + "retreat": n_retreat, } def _compute_pre_grasp_xpos( @@ -882,6 +1416,9 @@ def _plan( "Coordinated dual-arm planning is not supported by the cuRobo backend." ) state = context + left_start_qpos, right_start_qpos = self._resolve_dual_arm_start( + state, resources + ) ( object_initial_pose, object_target_pose, @@ -891,7 +1428,14 @@ def _plan( right_target_xpos, held_states, grasp_success, - ) = self._resolve_target(target, context, options, resources) + ) = self._resolve_target( + target, + context, + options, + resources, + left_start_qpos, + right_start_qpos, + ) left_held_state, right_held_state = held_states if not grasp_success.any(): logger.log_warning("CoordinatedPickment failed to resolve dual-arm grasps.") @@ -900,9 +1444,6 @@ def _plan( context, message="Failed to resolve dual-arm grasps.", ) - left_start_qpos, right_start_qpos = self._resolve_dual_arm_start( - state, resources - ) segments = self._compute_segment_lengths( request.motion_policy.sample_count, options ) @@ -1037,6 +1578,63 @@ def _plan( resources=resources, ) + release_trajectory = torch.empty( + (self.num_envs, 0, self.robot_dof), + dtype=torch.float32, + device=self.device, + ) + retreat_trajectory = torch.empty( + (self.num_envs, 0, self.robot_dof), + dtype=torch.float32, + device=self.device, + ) + if options.release: + release_trajectory = self._assemble_segment( + state, + self._repeat_qpos(left_target_qpos, segments["release"]), + self._repeat_qpos(right_target_qpos, segments["release"]), + self._interpolate_qpos( + resources.left_hand_close_qpos, + resources.left_hand_open_qpos, + segments["release"], + ), + self._interpolate_qpos( + resources.right_hand_close_qpos, + resources.right_hand_open_qpos, + segments["release"], + ), + resources=resources, + ) + retreat_delta = torch.tensor( + [0.0, 0.0, options.retreat_distance], + dtype=torch.float32, + device=self.device, + ) + left_retreat_xpos = translate_pose_world(left_target_xpos, retreat_delta) + right_retreat_xpos = translate_pose_world(right_target_xpos, retreat_delta) + success_mask, left_retreat_traj = self._plan_masked_arm_trajectory( + resources.left_arm.control_part, + left_target_qpos, + left_retreat_xpos.unsqueeze(1), + segments["retreat"], + success_mask, + ) + success_mask, right_retreat_traj = self._plan_masked_arm_trajectory( + resources.right_arm.control_part, + right_target_qpos, + right_retreat_xpos.unsqueeze(1), + segments["retreat"], + success_mask, + ) + retreat_trajectory = self._assemble_segment( + state, + left_retreat_traj, + right_retreat_traj, + self._repeat_qpos(resources.left_hand_open_qpos, segments["retreat"]), + self._repeat_qpos(resources.right_hand_open_qpos, segments["retreat"]), + resources=resources, + ) + full = torch.cat( [ approach_trajectory, @@ -1044,6 +1642,8 @@ def _plan( lift_trajectory, move_trajectory, hold_trajectory, + release_trajectory, + retreat_trajectory, ], dim=1, ) @@ -1068,8 +1668,12 @@ def _plan( ), expected_effects=StateDelta( held_object_updates={ - resources.left_task_state_key: left_held_object, - resources.right_task_state_key: right_held_object, + resources.left_task_state_key: ( + None if options.release else left_held_object + ), + resources.right_task_state_key: ( + None if options.release else right_held_object + ), }, ), segment_lengths={ @@ -1078,7 +1682,28 @@ def _plan( "lift": lift_trajectory.shape[1], "move": move_trajectory.shape[1], "hold": hold_trajectory.shape[1], + "release": release_trajectory.shape[1], + "retreat": retreat_trajectory.shape[1], }, + # The approach has three evenly spaced keyframes: current, + # pre-grasp, and grasp. Contact may move the coordinated object + # during the pre-grasp-to-grasp leg, so stop treating that + # expected self-motion as an external scene revision once both + # grippers reach the pre-grasp keyframe. Independent late-bound + # destination dependencies remain monitored for transport. + scene_dependency_monitor_until=( + {} + if ( + target.object_initial_pose is not None + or target.semantics.entity_id is None + ) + else { + target.semantics.entity_id: max( + 1, + math.ceil(approach_trajectory.shape[1] / 2), + ) + } + ), ) diff --git a/embodichain/lab/sim/atomic_actions/primitives/hand_over.py b/embodichain/lab/sim/atomic_actions/primitives/hand_over.py index 09e663cf4..80866c5c2 100644 --- a/embodichain/lab/sim/atomic_actions/primitives/hand_over.py +++ b/embodichain/lab/sim/atomic_actions/primitives/hand_over.py @@ -21,7 +21,7 @@ import math from collections.abc import Mapping from dataclasses import dataclass -from typing import ClassVar +from typing import ClassVar, Literal import torch @@ -29,7 +29,10 @@ from embodichain.utils.math import get_relative_rotation, pose_inv from embodichain.lab.sim.atomic_actions.affordance import AntipodalAffordance -from embodichain.lab.sim.atomic_actions.affordance_sampling import AffordanceSample +from embodichain.lab.sim.atomic_actions.affordance_sampling import ( + AffordancePoseCandidates, + AffordanceSample, +) from embodichain.lab.sim.atomic_actions.bindings import ( EndpointBinding, JointPositionTarget, @@ -106,13 +109,56 @@ class HandOverOptions(ActionOptions): hand_interp_steps: int = 10 """Waypoints used by every gripper open/close interpolation.""" + hold_steps: int = 4 + """Closed-hand waypoints used to settle a receiving grasp.""" + + retreat_steps: int = 20 + """Waypoints used while the source hand retreats after release.""" + + retreat_distance: float = 0.10 + """Distance retraced opposite the source TCP approach before lifting.""" + + receive_pick_object_part: Literal["center", "top", "bottom"] = "bottom" + """Object end selected by the receiving gripper for an existing hold.""" + + release_at_target: bool = True + """Whether the receiving hand places and releases after the transfer. + + When false, execution ends after the source hand opens and the receiving + resource remains the verified holder. This supports a semantic handover + followed by a later Place call without moving that workflow into Task Engine. + """ + + arm_selection: Literal["nearest", "bound"] = "nearest" + """How the transfer participant is selected. + + ``"nearest"`` preserves the low-level Atomic Action default for direct + callers. Semantic Task Program bindings should select ``"bound"`` so the + explicit ``source`` and ``destination`` resource slots are authoritative. + """ + def __post_init__(self) -> None: - for name in ("pre_grasp_distance", "lift_height"): + for name in ("pre_grasp_distance", "lift_height", "retreat_distance"): value = getattr(self, name) if not math.isfinite(value) or value < 0.0: raise ValueError(f"{name} must be finite and non-negative.") if self.hand_interp_steps < 1: raise ValueError("hand_interp_steps must be at least 1.") + for name in ("hold_steps", "retreat_steps"): + value = getattr(self, name) + if type(value) is not int or value < 0: + raise ValueError(f"{name} must be a non-negative integer.") + if self.retreat_steps < 2: + raise ValueError("retreat_steps must be at least 2.") + if self.receive_pick_object_part not in ("center", "top", "bottom"): + raise ValueError( + "receive_pick_object_part must be exactly 'center', 'top', or " + "'bottom'." + ) + if type(self.release_at_target) is not bool: + raise TypeError("release_at_target must be a bool.") + if self.arm_selection not in ("nearest", "bound"): + raise ValueError("arm_selection must be exactly 'nearest' or 'bound'.") @dataclass(frozen=True, slots=True, eq=False) @@ -149,7 +195,7 @@ class _DirectionalPlan: class HandOver(AtomicAction[HandOverGoal, HandOverOptions]): - """Pick an object with the nearer arm, hand it over, and place it. + """Pick an object with the nearer arm and transfer it to the other arm. For each environment, the action chooses the arm whose root link is closer to the observed object pose. It samples at most 1000 mesh-surface points and @@ -158,15 +204,21 @@ class HandOver(AtomicAction[HandOverGoal, HandOverOptions]): approaches point toward the object horizontally and tilt downward by 45 degrees. Otherwise both approaches are world-Z downward. - The first arm grasps the projected end of ``obj_longest_axis`` nearest its - current TCP; the receiving arm grasps the opposite end at the predicted - middle object pose. This keeps the two hands from selecting the same object - region regardless of whether a long object is standing or lying down. + For a free object, the first arm grasps the projected end of + ``obj_longest_axis`` nearest its current TCP. For an existing hold, that + source grasp is inherited from the preceding action and the receiving arm + uses the configured center, top, or bottom region at the predicted middle + object pose. A center request is filtered to the object's middle third; + top and bottom requests let Semantic Call look-ahead reserve the opposite + source end. After each grasp waypoint, subsequent EEF waypoints preserve that grasp - rotation and change translation only. In particular, placement first moves - strictly horizontally at the handover height and then lowers to the final - target pose before releasing the object. + rotation and change translation only. The released source TCP first + retraces its grasp approach before lifting. With + ``release_at_target=True``, the receiving arm additionally moves + horizontally at handover height, lowers to the final target pose, and + releases. Transfer-only mode stops with the receiving arm recorded as the + verified holder for a later Semantic Call. """ skill_id: ClassVar[str] = "hand_over" @@ -288,7 +340,7 @@ def _plan( request: ResolvedActionRequest[HandOverGoal, HandOverOptions], context: PlanningContext, ) -> ActionPlan: - """Plan the complete pick-up, handover, placement, and release.""" + """Plan pickup and transfer, with optional placement and release.""" goal = self.require_goal(request) options = request.skill_options resources = self._resolve_resources(request) @@ -303,6 +355,24 @@ def _plan( raise ValueError("HandOver requires an AntipodalAffordance.") goal.semantics.affordance.require_whole_object_mesh() + # A semantic handover is also the continuation point after an explicit + # Pick call. In that case the source attachment is already verified by + # TaskState and the action must transfer that attachment rather than + # silently attempting a second pickup. Keep the legacy unified route + # below for direct low-level callers that start with two free arms. + source_held = context.task.get_held_object(resources.first.task_state_key) + source_mask = context.task.held_object_mask(resources.first.task_state_key) + if source_held is not None and source_mask.any(): + if self._same_object(goal.semantics, source_held): + if options.release_at_target: + raise ValueError( + "HandOver cannot place an already-held object in the same " + "invocation; use a following Place call." + ) + return self._plan_existing_hold( + request, context, resources, source_held + ) + object_pose = _resolve_object_pose( goal.semantics, context, @@ -324,13 +394,23 @@ def _plan( ) first_root_pose = self._root_link_pose(resources.first.arm, context.env_ids) second_root_pose = self._root_link_pose(resources.second.arm, context.env_ids) - first_distance = torch.linalg.vector_norm( - object_pose[:, :3, 3] - first_root_pose[:, :3, 3], dim=1 - ) - second_distance = torch.linalg.vector_norm( - object_pose[:, :3, 3] - second_root_pose[:, :3, 3], dim=1 - ) - first_is_handover = first_distance <= second_distance + if options.arm_selection == "bound": + # Semantic bindings are authoritative: ``source`` acquires and + # ``destination`` receives. Do not silently invert an explicit + # request merely because the object starts nearer the other arm. + first_is_handover = torch.ones( + self.num_envs, + dtype=torch.bool, + device=self.device, + ) + else: + first_distance = torch.linalg.vector_norm( + object_pose[:, :3, 3] - first_root_pose[:, :3, 3], dim=1 + ) + second_distance = torch.linalg.vector_norm( + object_pose[:, :3, 3] - second_root_pose[:, :3, 3], dim=1 + ) + first_is_handover = first_distance <= second_distance # This unified action starts before pickup. Rows where either bound arm # already holds an object are therefore ineligible and remain at the @@ -466,8 +546,6 @@ def _plan( first_to_second.receive_grasp_xpos, second_to_first.handover_grasp_xpos, ) - # Both assignments use the same operation keys and sampling identity. - # Select row-local provenance from the assignment actually executed. affordance_samples = {} assignment = first_is_handover.cpu().tolist() for name, first_metadata in first_to_second.affordance_samples.items(): @@ -485,12 +563,41 @@ def _plan( semantics=goal.semantics, object_to_eef=first_object_to_eef, grasp_xpos=first_grasp_xpos, + env_mask=(None if options.release_at_target else ~first_is_handover), ) second_candidate = HeldObjectState( semantics=goal.semantics, object_to_eef=second_object_to_eef, grasp_xpos=second_grasp_xpos, + env_mask=(None if options.release_at_target else first_is_handover), ) + first_effect_candidate = HeldObjectState( + semantics=goal.semantics, + object_to_eef=first_object_to_eef, + grasp_xpos=first_grasp_xpos, + env_mask=eligible, + ) + second_effect_candidate = HeldObjectState( + semantics=goal.semantics, + object_to_eef=second_object_to_eef, + grasp_xpos=second_grasp_xpos, + env_mask=eligible, + ) + if options.release_at_target: + terminal_updates = { + resources.first.task_state_key: None, + resources.second.task_state_key: None, + } + elif options.arm_selection == "bound": + terminal_updates = { + resources.first.task_state_key: None, + resources.second.task_state_key: second_effect_candidate, + } + else: + terminal_updates = { + resources.first.task_state_key: first_candidate, + resources.second.task_state_key: second_candidate, + } return self.build_plan( request, @@ -502,15 +609,12 @@ def _plan( step_dt=context.require_control_dt(), ), expected_effects=StateDelta( - held_object_updates={ - resources.first.task_state_key: None, - resources.second.task_state_key: None, - }, + held_object_updates=terminal_updates, ), effect_candidates=StateDelta( held_object_updates={ - resources.first.task_state_key: first_candidate, - resources.second.task_state_key: second_candidate, + resources.first.task_state_key: first_effect_candidate, + resources.second.task_state_key: second_effect_candidate, }, ), diagnostics=PlannerDiagnostics( @@ -518,18 +622,515 @@ def _plan( metadata={"affordance_sample": affordance_samples}, ), segment_lengths=segment_lengths, + # The object may move from contact as soon as the pickup gripper + # starts closing. Keep dynamic-target monitoring active through + # the approach, but do not classify expected pickup motion as an + # external scene revision. scene_dependency_monitor_until=( {} if goal.semantics.entity_id is None - else { - goal.semantics.entity_id: ( - segment_lengths["pickup_approach"] - + segment_lengths["pickup_close"] - ) + else {goal.semantics.entity_id: segment_lengths["pickup_approach"]} + ), + ) + + def _plan_existing_hold( + self, + request: ResolvedActionRequest[HandOverGoal, HandOverOptions], + context: PlanningContext, + resources: _HandOverResources, + held: HeldObjectState, + ) -> ActionPlan: + """Transfer a verified source attachment to the destination hand. + + This is the canonical continuation used by ``Pick -> HandOver``. It + deliberately lives in the Atomic Action so Task Engine never owns + grasp poses, hand timing, or a second physical execution loop. + """ + goal = self.require_goal(request) + options = request.skill_options + source_mask = context.task.exclusive_held_object_mask( + resources.first.task_state_key + ) + destination_mask = context.task.held_object_mask( + resources.second.task_state_key + ) + eligible = source_mask & ~destination_mask + self._report_waypoint_failure( + context, + "existing_source_attachment", + ~source_mask, + "source participant does not own the requested object", + ) + self._report_waypoint_failure( + context, + "destination_unoccupied", + destination_mask, + "destination participant already holds an object", + ) + if not eligible.any(): + return self.failed_plan( + request, + context, + message=( + "HandOver requires an exclusive source attachment and an " + "unoccupied destination." + ), + ) + + source_start_qpos = context.last_qpos[:, list(resources.first.arm.joint_ids)] + destination_start_qpos = context.last_qpos[ + :, list(resources.second.arm.joint_ids) + ] + source_object_to_eef = held.object_to_eef.to( + device=self.device, dtype=torch.float32 + ) + if source_object_to_eef.dim() == 2: + source_object_to_eef = source_object_to_eef.unsqueeze(0).expand( + self.num_envs, -1, -1 + ) + source_eef = self.robot.compute_fk( + qpos=source_start_qpos, + name=resources.first.arm.control_part, + to_matrix=True, + ) + current_object_pose = torch.bmm(source_eef, pose_inv(source_object_to_eef)) + + source_root = self._root_link_pose(resources.first.arm, context.env_ids) + destination_root = self._root_link_pose( + resources.second.arm, + context.env_ids, + ) + # A continuation transfer derives its shared-workspace coordinate from + # the two bound arm roots, just like the unified free-arm route. The + # configured final target supplies only a safe absolute height because + # this mode deliberately leaves the destination holding the object. + # This keeps both transfer directions reachable without task-owned arm + # poses or direction-specific provider constants. + exchange_pose = current_object_pose.clone() + target_pose = resolve_batched_pose( + resolve_pose_goal( + goal.target_pose, + context, + name="handover_exchange_pose", + ), + num_envs=self.num_envs, + device=self.device, + name="handover_exchange_pose", + ) + exchange_pose[:, 2, 3] = torch.maximum( + current_object_pose[:, 2, 3], + target_pose[:, 2, 3], + ) + exchange_pose = self._middle_object_pose( + exchange_pose, + source_root, + destination_root, + ) + exchange_pose[:, :3, :3] = current_object_pose[:, :3, :3] + + source_exchange_eef = torch.bmm(exchange_pose, source_object_to_eef) + destination_eef = self.robot.compute_fk( + qpos=destination_start_qpos, + name=resources.second.arm.control_part, + to_matrix=True, + ) + # Approach diagonally from the receiver's side of the embodiment. A + # top-down receiver places two bulky parallel grippers in the same + # vertical envelope and can squeeze the object out while the source + # opens. Root-to-root direction is stable, robot-generic role geometry + # and reproduces the successful inward approach for either transfer + # direction without embedding left/right names. + approach_direction, approach_direction_valid = ( + self._downward_diagonal_approach_direction( + destination_root[:, :3, 3], + source_root[:, :3, 3], + ) + ) + + affordance = goal.semantics.affordance + assert isinstance(affordance, AntipodalAffordance) + longest_axis = affordance.get_object_longest_axis( + exchange_pose, + max_points=self._SURFACE_POINT_COUNT, + ) + receive_center_axis: torch.Tensor | None = None + if options.receive_pick_object_part != "center": + local_axis = exchange_pose.new_tensor([0.0, 0.0, 1.0]) + receive_axis: torch.Tensor | None = torch.matmul( + exchange_pose[:, :3, :3], local_axis + ) + receive_positive = torch.full( + (self.num_envs,), + options.receive_pick_object_part == "top", + dtype=torch.bool, + device=self.device, + ) + else: + # Ask the grasp service for all collision-free candidates, then + # retain only candidates through the object's middle third. The + # grasp-service axis selector can choose only one outer end, so it + # cannot represent a true center grasp by itself. + receive_axis = None + receive_center_axis = longest_axis + receive_positive = torch.ones( + self.num_envs, dtype=torch.bool, device=self.device + ) + destination_sample = self._resolve_grasp( + affordance, + exchange_pose, + approach_direction, + resources.second.hand.target_id, + obj_longest_axis=receive_axis, + is_positive_part=receive_positive, + center_axis=receive_center_axis, + context=context, + sample_key=(request.invocation_id or self.skill_id) + ":receive_grasp", + ) + destination_grasp = destination_sample.poses + grasp_success = destination_sample.success + destination_pre_grasp = translate_pose_world( + destination_grasp, + -destination_grasp[:, :3, 2] * options.pre_grasp_distance, + ) + destination_object_to_eef = torch.bmm( + pose_inv(exchange_pose), destination_grasp + ) + source_retreat_waypoints = self._source_retreat_waypoints( + source_exchange_eef, + destination_grasp, + source_fallback=source_eef, + destination_fallback=destination_eef, + retreat_distance=options.retreat_distance, + lift_height=options.lift_height, + ) + + lengths = self._compute_existing_hold_segment_lengths( + request.motion_policy.sample_count, + options, + ) + success = ( + normalize_success_mask( + grasp_success, + num_envs=self.num_envs, + device=self.device, + name="HandOver receiving-grasp success", + ) + & approach_direction_valid + & eligible + ) + self._report_waypoint_failure( + context, + "receive_approach_direction", + eligible & ~approach_direction_valid, + "source and destination roots have no horizontal separation", + ) + self._report_waypoint_failure( + context, + "receive_grasp", + eligible & ~success, + "no finite receiving grasp was found", + ) + + phase_success, source_transfer = plan_named_arm_trajectory( + self.motion_generator, + resources.first.arm.control_part, + source_start_qpos, + source_exchange_eef.unsqueeze(1), + lengths["transfer"], + request.motion_policy, + context.control_dt, + ) + success &= normalize_success_mask( + phase_success, + num_envs=self.num_envs, + device=self.device, + name="HandOver existing-hold source transfer success", + ) + source_hold_qpos = source_transfer[:, -1] + phase_success, destination_approach = plan_named_arm_trajectory( + self.motion_generator, + resources.second.arm.control_part, + destination_start_qpos, + torch.stack((destination_pre_grasp, destination_grasp), dim=1), + lengths["approach"], + request.motion_policy, + context.control_dt, + ) + success &= normalize_success_mask( + phase_success, + num_envs=self.num_envs, + device=self.device, + name="HandOver existing-hold destination approach success", + ) + destination_hold_qpos = destination_approach[:, -1] + phase_success, source_retreat = plan_named_arm_trajectory( + self.motion_generator, + resources.first.arm.control_part, + source_hold_qpos, + source_retreat_waypoints, + lengths["retreat"], + request.motion_policy, + context.control_dt, + ) + success &= normalize_success_mask( + phase_success, + num_envs=self.num_envs, + device=self.device, + name="HandOver existing-hold source retreat success", + ) + + # An existing hold is already physically established. Preserve the + # observed source-hand closure until the release phase instead of + # commanding a configured grasp setpoint that may be tighter than the + # contact-supported pose. + source_hand_qpos = context.last_qpos[:, list(resources.first.hand.joint_ids)] + + segment_values: list[tuple[str, torch.Tensor]] = [ + ( + "transfer", + self._assemble_segment( + context, + source_transfer, + repeat_qpos(destination_start_qpos, lengths["transfer"]), + repeat_qpos(source_hand_qpos, lengths["transfer"]), + repeat_qpos(resources.second.hand_open_qpos, lengths["transfer"]), + resources.first, + resources.second, + ), + ), + ( + "receive_approach", + self._assemble_segment( + context, + repeat_qpos(source_hold_qpos, lengths["approach"]), + destination_approach, + repeat_qpos(source_hand_qpos, lengths["approach"]), + repeat_qpos(resources.second.hand_open_qpos, lengths["approach"]), + resources.first, + resources.second, + ), + ), + ( + "receive_close", + self._assemble_segment( + context, + repeat_qpos(source_hold_qpos, lengths["close"]), + repeat_qpos(destination_hold_qpos, lengths["close"]), + repeat_qpos(source_hand_qpos, lengths["close"]), + interpolate_hand_qpos( + resources.second.hand_open_qpos, + resources.second.hand_grasp_qpos, + n_waypoints=lengths["close"], + ), + resources.first, + resources.second, + ), + ), + ] + if lengths["hold"]: + segment_values.append( + ( + "receive_hold", + self._assemble_segment( + context, + repeat_qpos(source_hold_qpos, lengths["hold"]), + repeat_qpos(destination_hold_qpos, lengths["hold"]), + repeat_qpos(source_hand_qpos, lengths["hold"]), + repeat_qpos(resources.second.hand_grasp_qpos, lengths["hold"]), + resources.first, + resources.second, + ), + ) + ) + segment_values.extend( + ( + ( + "handover_release", + self._assemble_segment( + context, + repeat_qpos(source_hold_qpos, lengths["release"]), + repeat_qpos(destination_hold_qpos, lengths["release"]), + interpolate_hand_qpos( + source_hand_qpos, + resources.first.hand_open_qpos, + n_waypoints=lengths["release"], + ), + repeat_qpos( + resources.second.hand_grasp_qpos, lengths["release"] + ), + resources.first, + resources.second, + ), + ), + ( + "source_retreat", + self._assemble_segment( + context, + source_retreat, + repeat_qpos(destination_hold_qpos, lengths["retreat"]), + repeat_qpos(resources.first.hand_open_qpos, lengths["retreat"]), + repeat_qpos( + resources.second.hand_grasp_qpos, lengths["retreat"] + ), + resources.first, + resources.second, + ), + ), + ) + ) + trajectory = torch.cat([value for _, value in segment_values], dim=1) + received = HeldObjectState( + semantics=held.semantics, + object_to_eef=destination_object_to_eef, + grasp_xpos=destination_grasp, + env_mask=eligible, + ) + return self.build_plan( + request, + context, + success=success, + trajectory=TimedTrajectory.from_uniform_step( + trajectory, + env_ids=context.env_ids, + step_dt=context.require_control_dt(), + ), + expected_effects=StateDelta( + held_object_updates={ + resources.first.task_state_key: None, + resources.second.task_state_key: received, } ), + effect_candidates=StateDelta( + held_object_updates={ + resources.first.task_state_key: held, + resources.second.task_state_key: received, + } + ), + diagnostics=PlannerDiagnostics( + backend=self.planning_services.planner_name, + metadata={ + "affordance_sample": { + "receive_grasp": { + **destination_sample.metadata, + "control_parts": [resources.second.arm.control_part] + * self.num_envs, + } + } + }, + ), + segment_lengths={name: value.shape[1] for name, value in segment_values}, + scene_dependency_monitor_until={ + entity_id: 0 + for entity_id in self._scene_dependencies(request) + if entity_id == goal.semantics.entity_id + }, ) + @staticmethod + def _source_retreat_waypoints( + source_exchange_eef: torch.Tensor, + destination_grasp: torch.Tensor, + *, + source_fallback: torch.Tensor, + destination_fallback: torch.Tensor, + retreat_distance: float, + lift_height: float, + ) -> torch.Tensor: + """Retrace the source grasp approach before lifting its open TCP. + + Moving along the line between the two TCP origins is not generally a + valid withdrawal direction: for a top grasp it sweeps the open source + fingers sideways through the transferred object. TCP ``-z`` is the + inverse of the source grasp approach, so intermediate Cartesian + waypoints on that ray clear the fingers before adding world-up + clearance. Root separation remains only a degenerate-pose fallback. + """ + if source_exchange_eef.shape != destination_grasp.shape: + raise ValueError( + "Source exchange and destination grasp poses must have matching " + "shapes." + ) + if source_fallback.shape != source_exchange_eef.shape: + raise ValueError("Source fallback poses must match exchange poses.") + if destination_fallback.shape != destination_grasp.shape: + raise ValueError("Destination fallback poses must match grasp poses.") + + direction = -source_exchange_eef[:, :3, 2] + fallback = source_fallback[:, :3, 3] - destination_fallback[:, :3, 3] + fallback[:, 2] = 0.0 + direction_norm = torch.linalg.vector_norm(direction, dim=1, keepdim=True) + fallback_norm = torch.linalg.vector_norm(fallback, dim=1, keepdim=True) + direction = torch.where( + direction_norm > 1.0e-6, + direction, + torch.where( + fallback_norm > 1.0e-6, + fallback, + direction.new_tensor([1.0, 0.0, 0.0]).expand_as(direction), + ), + ) + direction = direction / torch.linalg.vector_norm( + direction, dim=1, keepdim=True + ).clamp_min(1.0e-6) + + retreat_fractions = source_exchange_eef.new_tensor([1.0 / 3.0, 2.0 / 3.0, 1.0]) + retreat = source_exchange_eef[:, None].repeat(1, 3, 1, 1) + retreat[:, :, :3, 3] += ( + direction[:, None] * retreat_fractions[None, :, None] * retreat_distance + ) + lifted = retreat[:, -1:].repeat(1, 2, 1, 1) + lifted[:, :, 2, 3] += lifted.new_tensor([0.5, 1.0])[None] * lift_height + return torch.cat((retreat, lifted), dim=1) + + @staticmethod + def _same_object( + requested: object, + held: HeldObjectState, + ) -> bool: + """Return whether semantic object identity matches a held relation.""" + requested_entity_id = getattr(requested, "entity_id", None) + held_entity_id = held.semantics.entity_id + if requested_entity_id is not None and held_entity_id is not None: + return requested_entity_id == held_entity_id + requested_entity = getattr(requested, "entity", None) + held_entity = held.semantics.entity + if requested_entity is not None and held_entity is not None: + return requested_entity is held_entity + requested_label = getattr(requested, "label", None) + return bool(requested_label) and requested_label == held.semantics.label + + @staticmethod + def _compute_existing_hold_segment_lengths( + sample_count: int, + options: HandOverOptions, + ) -> dict[str, int]: + """Split one existing-hold transfer into motion and hand phases.""" + close = max(2, options.hand_interp_steps) + release = max(2, options.hand_interp_steps) + hold = options.hold_steps + retreat = max(2, options.retreat_steps) + reserved = close + release + hold + retreat + remaining = sample_count - reserved + if remaining < 4: + raise ValueError( + "Not enough HandOver waypoints for an existing held-object " + "transfer; increase sample_count or reduce hand phases." + ) + transfer = max(2, remaining // 2) + approach = remaining - transfer + if approach < 2: + raise ValueError( + "Not enough HandOver waypoints for the receiving approach." + ) + return { + "transfer": transfer, + "approach": approach, + "close": close, + "hold": hold, + "release": release, + "retreat": retreat, + } + def _find_symmetric_nearest_xpos( self, target_xpos: torch.Tensor, reference_xpos: torch.Tensor ) -> torch.Tensor: @@ -631,8 +1232,9 @@ def _plan_direction( sample_key=(request.invocation_id or self.skill_id) + ":pickup_grasp", ) handover_grasp_success = handover_sample.success + handover_grasp = handover_sample.poses handover_grasp = self._find_symmetric_nearest_xpos( - handover_sample.poses, handover_start_eef + handover_grasp, handover_start_eef ) handover_pre_grasp = translate_pose_world( handover_grasp, @@ -683,8 +1285,9 @@ def _plan_direction( sample_key=(request.invocation_id or self.skill_id) + ":receive_grasp", ) receive_grasp_success = receive_sample.success + receive_grasp = receive_sample.poses receive_grasp = self._find_symmetric_nearest_xpos( - receive_sample.poses, receive_start_eef + receive_grasp, receive_start_eef ) receive_pre_grasp = translate_pose_world( @@ -695,22 +1298,30 @@ def _plan_direction( pose_inv(middle_object_pose), receive_grasp, ) - placed_object_pose = final_object_pose.clone() - placed_object_pose[:, :3, :3] = middle_object_pose[:, :3, :3] - above_object_pose = placed_object_pose.clone() - # Move to the target's horizontal coordinates while preserving the - # middle handover height exactly. The following target performs the - # only vertical motion and reaches the requested final object pose. - above_object_pose[:, 2, 3] = middle_object_pose[:, 2, 3] - lowering_direction_valid = ( - above_object_pose[:, 2, 3] - placed_object_pose[:, 2, 3] > 1.0e-6 - ) - receive_above_eef = torch.bmm(above_object_pose, receive_object_to_eef) - receive_final_eef = torch.bmm(placed_object_pose, receive_object_to_eef) - # Likewise, receiving-grasp through final lowering reuses the same EEF - # rotation and changes translation only. - receive_above_eef[:, :3, :3] = receive_grasp[:, :3, :3] - receive_final_eef[:, :3, :3] = receive_grasp[:, :3, :3] + lowering_direction_valid = torch.ones( + self.num_envs, + dtype=torch.bool, + device=self.device, + ) + receive_above_eef: torch.Tensor | None = None + receive_final_eef: torch.Tensor | None = None + if options.release_at_target: + placed_object_pose = final_object_pose.clone() + placed_object_pose[:, :3, :3] = middle_object_pose[:, :3, :3] + above_object_pose = placed_object_pose.clone() + # Move to the target's horizontal coordinates while preserving the + # middle handover height exactly. The following target performs the + # only vertical motion and reaches the requested final object pose. + above_object_pose[:, 2, 3] = middle_object_pose[:, 2, 3] + lowering_direction_valid = ( + above_object_pose[:, 2, 3] - placed_object_pose[:, 2, 3] > 1.0e-6 + ) + receive_above_eef = torch.bmm(above_object_pose, receive_object_to_eef) + receive_final_eef = torch.bmm(placed_object_pose, receive_object_to_eef) + # Likewise, receiving-grasp through final lowering reuses the same EEF + # rotation and changes translation only. + receive_above_eef[:, :3, :3] = receive_grasp[:, :3, :3] + receive_final_eef[:, :3, :3] = receive_grasp[:, :3, :3] self._report_waypoint_failure( context, "pickup_grasp", @@ -739,12 +1350,13 @@ def _plan_direction( "no finite grasp candidate on the opposite object end for arm " f"{receive.arm.control_part!r}", ) - self._report_waypoint_failure( - context, - "target_final", - active_mask & ~lowering_direction_valid, - "final target is not below the horizontal-transfer height", - ) + if options.release_at_target: + self._report_waypoint_failure( + context, + "target_final", + active_mask & ~lowering_direction_valid, + "final target is not below the horizontal-transfer height", + ) success = ( handover_direction_valid @@ -843,33 +1455,81 @@ def _plan_direction( success &= receive_approach_success receive_grasp_qpos = receive_approach[:, -1] - placement_targets = torch.stack([receive_above_eef, receive_final_eef], dim=1) - phase_success, receive_place = plan_named_arm_trajectory( - self.motion_generator, - receive.arm.control_part, - receive_grasp_qpos, - placement_targets, - segment_lengths["place"], - request.motion_policy, - context.control_dt, - ) - placement_success = normalize_success_mask( - phase_success, - num_envs=self.num_envs, - device=self.device, - name="HandOver placement success", - ) - self._report_phase_failure( - context, - phase_name="place", - waypoint_names=("target_above", "target_final"), - target_poses=placement_targets, - start_qpos=receive_grasp_qpos, - arm=receive.arm, - failed_mask=active_mask & ~placement_success, - ) - success &= placement_success - receive_final_qpos = receive_place[:, -1] + source_retreat: torch.Tensor | None = None + if not options.release_at_target: + source_retreat_waypoints = self._source_retreat_waypoints( + handover_middle_eef, + receive_grasp, + source_fallback=handover_middle_eef, + destination_fallback=receive_grasp, + retreat_distance=options.retreat_distance, + lift_height=options.lift_height, + ) + phase_success, source_retreat = plan_named_arm_trajectory( + self.motion_generator, + handover.arm.control_part, + handover_middle_qpos, + source_retreat_waypoints, + segment_lengths["source_retreat"], + request.motion_policy, + context.control_dt, + ) + source_retreat_success = normalize_success_mask( + phase_success, + num_envs=self.num_envs, + device=self.device, + name="HandOver source-retreat success", + ) + self._report_phase_failure( + context, + phase_name="source_retreat", + waypoint_names=( + "source_withdraw_1", + "source_withdraw_2", + "source_withdraw_3", + "source_lift_1", + "source_lift_2", + ), + target_poses=source_retreat_waypoints, + start_qpos=handover_middle_qpos, + arm=handover.arm, + failed_mask=active_mask & ~source_retreat_success, + ) + success &= source_retreat_success + + receive_place: torch.Tensor | None = None + receive_final_qpos: torch.Tensor | None = None + if options.release_at_target: + assert receive_above_eef is not None and receive_final_eef is not None + placement_targets = torch.stack( + [receive_above_eef, receive_final_eef], dim=1 + ) + phase_success, receive_place = plan_named_arm_trajectory( + self.motion_generator, + receive.arm.control_part, + receive_grasp_qpos, + placement_targets, + segment_lengths["place"], + request.motion_policy, + context.control_dt, + ) + placement_success = normalize_success_mask( + phase_success, + num_envs=self.num_envs, + device=self.device, + name="HandOver placement success", + ) + self._report_phase_failure( + context, + phase_name="place", + waypoint_names=("target_above", "target_final"), + target_poses=placement_targets, + start_qpos=receive_grasp_qpos, + arm=receive.arm, + failed_mask=active_mask & ~placement_success, + ) + success &= placement_success + receive_final_qpos = receive_place[:, -1] segments = [ self._assemble_segment( @@ -953,31 +1613,60 @@ def _plan_direction( handover, receive, ), - self._assemble_segment( - state, - repeat_qpos(handover_middle_qpos, segment_lengths["place"]), - receive_place, - repeat_qpos(handover.hand_open_qpos, segment_lengths["place"]), - repeat_qpos(receive.hand_grasp_qpos, segment_lengths["place"]), - handover, - receive, - ), - self._assemble_segment( - state, - repeat_qpos(handover_middle_qpos, segment_lengths["receive_release"]), - repeat_qpos(receive_final_qpos, segment_lengths["receive_release"]), - repeat_qpos( - handover.hand_open_qpos, segment_lengths["receive_release"] - ), - interpolate_hand_qpos( - receive.hand_grasp_qpos, - receive.hand_open_qpos, - n_waypoints=segment_lengths["receive_release"], - ), - handover, - receive, - ), ] + if options.release_at_target: + assert receive_place is not None and receive_final_qpos is not None + segments.extend( + ( + self._assemble_segment( + state, + repeat_qpos(handover_middle_qpos, segment_lengths["place"]), + receive_place, + repeat_qpos(handover.hand_open_qpos, segment_lengths["place"]), + repeat_qpos(receive.hand_grasp_qpos, segment_lengths["place"]), + handover, + receive, + ), + self._assemble_segment( + state, + repeat_qpos( + handover_middle_qpos, + segment_lengths["receive_release"], + ), + repeat_qpos( + receive_final_qpos, + segment_lengths["receive_release"], + ), + repeat_qpos( + handover.hand_open_qpos, + segment_lengths["receive_release"], + ), + interpolate_hand_qpos( + receive.hand_grasp_qpos, + receive.hand_open_qpos, + n_waypoints=segment_lengths["receive_release"], + ), + handover, + receive, + ), + ) + ) + elif source_retreat is not None: + segments.append( + self._assemble_segment( + state, + source_retreat, + repeat_qpos(receive_grasp_qpos, segment_lengths["source_retreat"]), + repeat_qpos( + handover.hand_open_qpos, segment_lengths["source_retreat"] + ), + repeat_qpos( + receive.hand_grasp_qpos, segment_lengths["source_retreat"] + ), + handover, + receive, + ) + ) trajectory = torch.cat(segments, dim=1) actual_lengths = { name: segment.shape[1] @@ -1132,12 +1821,13 @@ def _resolve_grasp( approach_direction: torch.Tensor, grasp_target_id: str, *, - obj_longest_axis: torch.Tensor, + obj_longest_axis: torch.Tensor | None, is_positive_part: torch.Tensor, + center_axis: torch.Tensor | None = None, context: PlanningContext, sample_key: str, ) -> AffordanceSample: - """Sample a grasp branch on one projected end of the object.""" + """Sample one grasp after applying the requested object-region filter.""" if object_pose.shape != (self.num_envs, 4, 4): raise ValueError( "HandOver grasp object_pose must have shape " @@ -1148,10 +1838,17 @@ def _resolve_grasp( "HandOver grasp approach_direction must have shape " f"({self.num_envs}, 3)." ) - if obj_longest_axis.shape != (self.num_envs, 3): + if obj_longest_axis is not None and obj_longest_axis.shape != ( + self.num_envs, + 3, + ): raise ValueError( f"HandOver obj_longest_axis must have shape ({self.num_envs}, 3)." ) + if center_axis is not None and center_axis.shape != (self.num_envs, 3): + raise ValueError( + f"HandOver center_axis must have shape ({self.num_envs}, 3)." + ) if is_positive_part.dtype != torch.bool or is_positive_part.shape != ( self.num_envs, ): @@ -1168,6 +1865,13 @@ def _resolve_grasp( obj_longest_axis=obj_longest_axis, is_positive_part=is_positive_part, ) + if center_axis is not None: + candidates = self._center_grasp_candidates( + affordance, + object_pose, + center_axis, + candidates, + ) return affordance.sample_candidates( candidates, sampling=context.affordance_sampling, @@ -1176,6 +1880,45 @@ def _resolve_grasp( reference_poses=object_pose, ) + def _center_grasp_candidates( + self, + affordance: AntipodalAffordance, + object_pose: torch.Tensor, + center_axis: torch.Tensor, + candidates: AffordancePoseCandidates, + ) -> AffordancePoseCandidates: + """Keep grasp centers in the middle third of the object's long axis.""" + vertices = affordance.mesh_vertices + if vertices is None: + raise ValueError("Center HandOver grasp selection requires mesh vertices.") + axis = center_axis.to(device=self.device, dtype=torch.float32) + axis_norm = torch.linalg.vector_norm(axis, dim=1, keepdim=True) + if not torch.isfinite(axis).all() or (axis_norm <= 1.0e-8).any(): + raise ValueError("HandOver center_axis must be finite and non-zero.") + axis = axis / axis_norm + vertices = vertices.to(device=self.device, dtype=torch.float32) + world_vertices = ( + torch.matmul(vertices[None], object_pose[:, :3, :3].transpose(1, 2)) + + object_pose[:, None, :3, 3] + ) + projections = torch.sum(world_vertices * axis[:, None], dim=2) + span = projections.max(dim=1).values - projections.min(dim=1).values + if not torch.isfinite(span).all() or (span <= 1.0e-8).any(): + raise ValueError("Center HandOver grasp selection requires finite extent.") + lower = projections.min(dim=1).values + span / 3.0 + upper = projections.max(dim=1).values - span / 3.0 + candidate_projections = torch.sum( + candidates.poses[:, :, :3, 3] * axis[:, None], dim=2 + ) + middle = (candidate_projections >= lower[:, None]) & ( + candidate_projections <= upper[:, None] + ) + return AffordancePoseCandidates( + poses=candidates.poses, + costs=candidates.costs, + valid=candidates.valid & middle, + ) + @staticmethod def _downward_diagonal_approach_direction( start_position: torch.Tensor, @@ -1228,27 +1971,37 @@ def _compute_segment_lengths( sample_count: int, options: HandOverOptions, ) -> dict[str, int]: - """Split the sample budget across four arm and four hand phases.""" + """Split the sample budget across enabled arm and hand phases.""" hand_count = options.hand_interp_steps - motion_budget = sample_count - 4 * hand_count - if motion_budget < 8: + hand_phase_count = 4 if options.release_at_target else 3 + motion_phase_count = 4 + motion_budget = sample_count - hand_phase_count * hand_count + if motion_budget < 2 * motion_phase_count: raise ValueError( "Not enough HandOver waypoints. Increase sample_count or decrease " "hand_interp_steps." ) - motion_counts = [motion_budget // 4] * 4 - for index in range(motion_budget % 4): + motion_counts = [motion_budget // motion_phase_count] * motion_phase_count + for index in range(motion_budget % motion_phase_count): motion_counts[index] += 1 - return { + result = { "pickup_approach": motion_counts[0], "pickup_close": hand_count, "pickup_transport": motion_counts[1], "receive_approach": motion_counts[2], "receive_close": hand_count, "handover_release": hand_count, - "place": motion_counts[3], - "receive_release": hand_count, } + if options.release_at_target: + result.update( + { + "place": motion_counts[3], + "receive_release": hand_count, + } + ) + else: + result["source_retreat"] = motion_counts[3] + return result @staticmethod def _assemble_segment( diff --git a/embodichain/lab/sim/atomic_actions/primitives/move_held_object.py b/embodichain/lab/sim/atomic_actions/primitives/move_held_object.py index 695a0f927..a1bfbf26b 100644 --- a/embodichain/lab/sim/atomic_actions/primitives/move_held_object.py +++ b/embodichain/lab/sim/atomic_actions/primitives/move_held_object.py @@ -19,6 +19,7 @@ from __future__ import annotations from dataclasses import dataclass +import math from typing import ClassVar import torch @@ -26,7 +27,6 @@ from embodichain.lab.sim.atomic_actions.primitives._helpers import ( arm_qpos_from_state, require_shared_task_state_key, - resolve_object_target, ) from embodichain.lab.sim.atomic_actions.bindings import JointPositionTarget from embodichain.lab.sim.atomic_actions.control import ( @@ -43,7 +43,8 @@ ActionOptions, ResolvedActionRequest, ) -from embodichain.lab.sim.atomic_actions.plans import ActionPlan +from embodichain.lab.sim.atomic_actions.plans import ActionPlan, PlannerDiagnostics +from embodichain.lab.sim.motion.planners.utils import PlanResult from embodichain.lab.sim.atomic_actions.requirements import ( CARTESIAN_POSE_CAPABILITY, FORWARD_KINEMATICS_CAPABILITY, @@ -52,6 +53,7 @@ from embodichain.lab.sim.atomic_actions.state import PlanningContext from embodichain.lab.sim.atomic_actions.trajectory_ops import ( build_pose_plan_states, + resolve_pose_target, to_full_robot_trajectory, ) from embodichain.lab.sim.atomic_actions.primitives._binding_contracts import ( @@ -64,13 +66,27 @@ class HeldObjectPoseGoal: """Desired pose for the object held by this action's control part.""" object_target_pose: PoseGoalValue - """Target object pose, shape ``(4, 4)`` or ``(num_envs, 4, 4)``.""" + """Object target pose or ordered waypoints. + + Accepts ``(4, 4)``, ``(num_envs, 4, 4)`` or + ``(num_envs, n_waypoint, 4, 4)``; every waypoint retains the held grasp. + """ + + world_yaw_free: bool = False + """Permit world-Z yaw changes at the final waypoint, retaining its position. + + Intermediate waypoints remain exact. Planning tries the nominal heading + first, then seven 45-degree heading alternatives; each complete path must + pass the motion generator's normal checks. This is not an exhaustive search. + """ def __post_init__(self) -> None: + if type(self.world_yaw_free) is not bool: + raise TypeError("world_yaw_free must be a boolean.") validate_pose_goal( self.object_target_pose, "object_target_pose", - allow_waypoints=False, + allow_waypoints=True, ) @@ -80,9 +96,10 @@ class MoveHeldObjectOptions(ActionOptions): class MoveHeldObject(AtomicAction[HeldObjectPoseGoal, MoveHeldObjectOptions]): - """Move the held object to the exact target object pose with a closed hand. + """Move the held object through exact target poses with a closed hand. - The requested object orientation is preserved exactly. Callers that need a + Requested orientations are exact unless final world yaw is explicitly free. + Callers that need a transport orientation must encode it in :class:`HeldObjectPoseGoal`; this action never substitutes an implicit end-effector orientation. """ @@ -146,7 +163,7 @@ def _plan( context, message="Held object is not exclusive to the control part.", ) - object_target_pose = resolve_object_target( + object_target_pose = resolve_pose_target( resolve_pose_goal( target.object_target_pose, context, @@ -161,18 +178,47 @@ def _plan( ) if object_to_eef.shape == (4, 4): object_to_eef = object_to_eef.unsqueeze(0).repeat(self.num_envs, 1, 1) - move_eef_xpos = torch.bmm(object_target_pose, object_to_eef) - - result = self.motion_generator.generate( - build_pose_plan_states(move_eef_xpos), - options=request.motion_policy.to_motion_gen_options( - start_qpos=start_arm_qpos, - control_part=control_part, - interpolation_dt=context.control_dt, - ), + if object_target_pose.ndim == 4: + object_to_eef = object_to_eef.unsqueeze(1) + motion_options = request.motion_policy.to_motion_gen_options( + start_qpos=start_arm_qpos, + control_part=control_part, + interpolation_dt=context.control_dt, ) + + def plan_target(poses: torch.Tensor) -> PlanResult: + return self.motion_generator.generate( + build_pose_plan_states(torch.matmul(poses, object_to_eef)), + options=motion_options, + ) + + result = plan_target(object_target_pose) assert isinstance(result.success, torch.Tensor) - assert result.positions is not None + selected_yaws = start_arm_qpos.new_zeros(self.num_envs) + attempts = 1 + if target.world_yaw_free: + for fraction in (0.25, -0.25, 0.5, -0.5, 0.75, -0.75, 1.0): + missing = eligible & ~result.success + if not missing.any(): + break + angle = math.pi * fraction + cosine, sine = math.cos(angle), math.sin(angle) + yaw = object_target_pose.new_tensor( + [[cosine, -sine, 0.0], [sine, cosine, 0.0], [0.0, 0.0, 1.0]] + ) + poses = object_target_pose.clone() + final_pose = poses[:, -1] if poses.ndim == 4 else poses + final_pose[:, :3, :3] = yaw @ final_pose[:, :3, :3] + candidate = plan_target(poses) + attempts += 1 + accepted = missing & candidate.success + if accepted.any(): + result = _merge_yaw_paths(result, candidate, accepted) + selected_yaws[accepted] = angle + if result.positions is None: + return self.failed_plan( + request, context, message="No feasible held-object transport path." + ) success = result.success & eligible base_qpos = state.last_qpos.clone() @@ -190,10 +236,53 @@ def _plan( context, success=success, trajectory=timed, + diagnostics=( + PlannerDiagnostics( + backend=self.planning_services.planner_name, + metadata={ + "world_yaw_offsets_rad": selected_yaws.tolist(), + "yaw_plan_attempts": attempts, + }, + ) + if target.world_yaw_free + else None + ), segment_lengths={"transport": timed.waypoint_count}, ) +def _merge_yaw_paths( + previous: PlanResult, candidate: PlanResult, rows: torch.Tensor +) -> PlanResult: + """Retain accepted rows while selecting newly feasible, independently timed paths.""" + assert candidate.positions is not None and candidate.dt is not None + if previous.positions is None: + return candidate + assert previous.dt is not None + count = max(previous.positions.shape[1], candidate.positions.shape[1]) + + def pad_positions(value: torch.Tensor) -> torch.Tensor: + return torch.cat( + [value, value[:, -1:].expand(-1, count - value.shape[1], -1)], dim=1 + ) + + # Zero-time terminal padding preserves each row's duration. The action + # recomputes derivatives on the control grid in to_full_robot_trajectory. + return PlanResult( + success=torch.where(rows, candidate.success, previous.success), + positions=torch.where( + rows[:, None, None], + pad_positions(candidate.positions), + pad_positions(previous.positions), + ), + dt=torch.where( + rows[:, None], + torch.nn.functional.pad(candidate.dt, (0, count - candidate.dt.shape[1])), + torch.nn.functional.pad(previous.dt, (0, count - previous.dt.shape[1])), + ), + ) + + __all__ = [ "HeldObjectPoseGoal", "MoveHeldObject", diff --git a/embodichain/lab/sim/atomic_actions/primitives/move_joints.py b/embodichain/lab/sim/atomic_actions/primitives/move_joints.py index b7196c652..58bd3a33d 100644 --- a/embodichain/lab/sim/atomic_actions/primitives/move_joints.py +++ b/embodichain/lab/sim/atomic_actions/primitives/move_joints.py @@ -82,7 +82,7 @@ class MoveJoints(AtomicAction[JointPositionGoal, MoveJointsOptions]): skill_id: ClassVar[str] = "move_joints" GoalType: ClassVar[type] = JointPositionGoal OptionsType: ClassVar[type] = MoveJointsOptions - agent_visible: ClassVar[bool] = False + agent_visible: ClassVar[bool] = True binding_contract: ClassVar[SkillBindingContract] = SkillBindingContract( slots=( make_motion_slot( diff --git a/embodichain/lab/sim/atomic_actions/primitives/pick_up.py b/embodichain/lab/sim/atomic_actions/primitives/pick_up.py index 80d0d983f..31b96c0b3 100644 --- a/embodichain/lab/sim/atomic_actions/primitives/pick_up.py +++ b/embodichain/lab/sim/atomic_actions/primitives/pick_up.py @@ -55,7 +55,6 @@ PoseGoalValue, SceneEntityPose, _resolve_object_pose, - collect_scene_dependencies, resolve_pose_goal, validate_pose_goal, ) @@ -175,6 +174,9 @@ class PickUpOptions(ActionOptions): pre_grasp_distance: float = 0.15 """Distance to offset back from the grasp pose along the approach direction.""" + grasp_commit_fraction: float = 1.0 + """Approach fraction after which contact motion no longer invalidates grasp.""" + approach_direction: torch.Tensor = torch.tensor([0, 0, -1], dtype=torch.float32) """World-frame direction from the pre-grasp pose to the grasp pose.""" @@ -211,6 +213,12 @@ def __post_init__(self) -> None: raise ValueError("lift_height must be non-negative.") if self.pre_grasp_distance < 0.0: raise ValueError("pre_grasp_distance must be non-negative.") + if isinstance(self.grasp_commit_fraction, bool) or not isinstance( + self.grasp_commit_fraction, (int, float) + ): + raise TypeError("grasp_commit_fraction must be a real number.") + if not 0.0 < self.grasp_commit_fraction <= 1.0: + raise ValueError("grasp_commit_fraction must be in (0, 1].") if self.approach_direction.shape != (3,): raise ValueError("approach_direction must have shape (3,).") if not torch.isfinite(self.approach_direction).all(): @@ -291,7 +299,7 @@ def _scene_dependencies( self, request: ResolvedActionRequest[GraspGoal, PickUpOptions], ) -> tuple[str, ...]: - """Include the semantic object when it has a stable scene identity.""" + """Monitor the acquired object and late-bound downstream targets.""" dependencies = set(super()._scene_dependencies(request)) entity_id = request.goal.semantics.entity_id # An explicit object pose is a scene-independent planning input. Do @@ -304,11 +312,6 @@ def _scene_dependencies( # live scene snapshot. if entity_id is not None and request.goal.object_pose is None: dependencies.add(entity_id) - dependencies.update( - collect_scene_dependencies( - request.skill_options.downstream_object_target_poses - ) - ) return tuple(sorted(dependencies)) def _get_full_pickup_trajectory( @@ -598,13 +601,20 @@ def _plan( ), ), segment_lengths=segment_lengths, - # Once the approach is dispatched the object can move because of - # contact or grasping. That self-induced motion must not look like - # an external dynamic-goal update. + # Once the approach is dispatched, contact can move the acquired + # object. Keep downstream late-bound targets monitored so a changed + # destination is resolved again before the action continues. scene_dependency_monitor_until=( {} if monitored_object_id is None - else {monitored_object_id: segment_lengths["approach"]} + else { + monitored_object_id: max( + 1, + math.ceil( + segment_lengths["approach"] * options.grasp_commit_fraction + ), + ) + } ), ) diff --git a/embodichain/lab/sim/atomic_actions/primitives/slide.py b/embodichain/lab/sim/atomic_actions/primitives/slide.py index 7bfc60800..85fa880af 100644 --- a/embodichain/lab/sim/atomic_actions/primitives/slide.py +++ b/embodichain/lab/sim/atomic_actions/primitives/slide.py @@ -19,7 +19,8 @@ from __future__ import annotations import math -from dataclasses import dataclass +from dataclasses import dataclass, field +from numbers import Real from typing import ClassVar, Literal import torch @@ -70,6 +71,48 @@ ) +@dataclass(frozen=True, slots=True) +class SlideJointTarget: + """Absolute prismatic coordinate, in metres, with a calibrated axis sign. + + Args: + articulation_id: Canonical articulation identity in the scene snapshot. + joint_name: Exact observed prismatic joint name. + position: Requested absolute joint position in metres. + axis_sign: Maps increasing joint position to the affordance axis (+1/-1). + tolerance: Position tolerance for an already-satisfied planning row. + """ + + articulation_id: str + joint_name: str + position: float + axis_sign: int = field(kw_only=True) + tolerance: float = 1.0e-4 + + def __post_init__(self) -> None: + for name in ("articulation_id", "joint_name"): + value = getattr(self, name) + if type(value) is not str or not value or value != value.strip(): + raise ValueError(f"{name} must be an exact non-empty identifier.") + for name in ("position", "tolerance"): + value = getattr(self, name) + if ( + isinstance(value, bool) + or not isinstance(value, Real) + or not math.isfinite(value) + ): + raise ValueError(f"{name} must be a finite real number.") + object.__setattr__(self, name, float(value)) + if ( + self.tolerance <= 0 + or type(self.axis_sign) is not int + or self.axis_sign not in (-1, 1) + ): + raise ValueError( + "Slide joint targets require positive tolerance and axis_sign +/-1." + ) + + @dataclass(frozen=True, slots=True, eq=False) class SlideGoal(ObjectActionGoal): """Translating articulation link described by a slide affordance.""" @@ -77,9 +120,22 @@ class SlideGoal(ObjectActionGoal): target_pose: PoseGoalValue """Link pose snapshot or late-bound stable scene-entity reference.""" + joint_target: SlideJointTarget | None = field(default=None, kw_only=True) + """Optional absolute joint intent; otherwise preserve fixed-distance Options. + + Joint-target mode resolves fresh row-local distances and the pull/push + direction during planning. Active rows must share one direction; already + satisfied rows hold their observed pose. This does not verify physical effect. + """ + def __post_init__(self) -> None: ObjectActionGoal.__post_init__(self) validate_pose_goal(self.target_pose, "target_pose", allow_waypoints=False) + if ( + self.joint_target is not None + and type(self.joint_target) is not SlideJointTarget + ): + raise TypeError("joint_target must be exactly SlideJointTarget or None.") @dataclass(frozen=True, slots=True, eq=False) @@ -98,6 +154,15 @@ class SlideOptions(ActionOptions): translation_distance: float = 0.15 """Distance traveled along the pull or push direction.""" + preshape_fraction: float = 0.0 + """Fraction from the bound open to grasp command during approach and reach.""" + + approach_along_grasp_axis: bool = False + """Approach along grasp Z instead of the rail axis; rail motion is unchanged.""" + + release_retreat_distance: float = 0.0 + """Distance opposite grasp Z while opening; zero holds the arm still.""" + def __post_init__(self) -> None: if self.direction not in ("pull", "push"): raise ValueError("direction must be either 'pull' or 'push'.") @@ -111,6 +176,18 @@ def __post_init__(self) -> None: raise ValueError("translation_distance must be finite.") if self.translation_distance <= 0.0: raise ValueError("translation_distance must be positive.") + for name in ("preshape_fraction", "release_retreat_distance"): + value = getattr(self, name) + if isinstance(value, bool) or not isinstance(value, (int, float)): + raise TypeError(f"{name} must be a real number.") + if not math.isfinite(value) or value < 0.0: + raise ValueError(f"{name} must be finite and non-negative.") + if self.preshape_fraction > 1.0: + raise ValueError("preshape_fraction must not exceed one.") + if type(self.approach_along_grasp_axis) is not bool: + raise TypeError("approach_along_grasp_axis must be a boolean.") + if self.release_retreat_distance > 0.0 and self.hand_interp_steps < 2: + raise ValueError("Release retreat requires hand_interp_steps >= 2.") class Slide(AtomicAction[SlideGoal, SlideOptions]): @@ -151,6 +228,56 @@ def _plan( affordance = self._require_slide_affordance(target.semantics) options = request.skill_options interpolation_dt = context.require_control_dt() + # Resolve late-bound scene targets before any already-satisfied fast + # path so invalid or low-confidence references fail at the goal + # boundary instead of being reported as a successful hold. + resolved_link_pose = resolve_pose_goal( + target.target_pose, + context, + name="target_pose", + ) + direction = options.direction + displacement = None + joint_valid = torch.ones( + context.batch_size, dtype=torch.bool, device=self.device + ) + reached = torch.zeros_like(joint_valid) + diagnostics: PlannerDiagnostics | None = None + if target.joint_target is not None: + displacement, joint_valid, reached = self._joint_displacement( + target.joint_target, affordance, context + ) + active = joint_valid & ~reached + diagnostics = PlannerDiagnostics( + backend=self.planning_services.planner_name, + metadata={ + "joint_target": { + "articulation_id": target.joint_target.articulation_id, + "joint_name": target.joint_target.joint_name, + "position": target.joint_target.position, + "already_satisfied": reached.detach().cpu().tolist(), + } + }, + ) + if not active.any(): + return self.build_plan( + request, + context, + success=reached, + trajectory=TimedTrajectory.from_uniform_step( + context.robot.qpos[:, None], + env_ids=context.env_ids, + step_dt=interpolation_dt, + ), + segment_lengths={"already_satisfied": 1}, + diagnostics=diagnostics, + ) + positive = displacement[active] > 0 + if positive.any() and not positive.all(): + raise ValueError( + "One Slide invocation requires the same direction for active joint-target rows." + ) + direction = "push" if positive.all() else "pull" binding = request.binding motion_target = binding.endpoint("primary", "motion").require_target( JointPositionTarget @@ -175,7 +302,7 @@ def _plan( ) link_pose = resolve_pose_target( - resolve_pose_goal(target.target_pose, context, name="target_pose"), + resolved_link_pose, num_envs=self.num_envs, device=self.device, ) @@ -208,25 +335,48 @@ def _plan( name="Slide grasp-pose success", ) if not grasp_success.any(): + if target.joint_target is not None and reached.any(): + return self.build_plan( + request, + context, + success=reached, + trajectory=TimedTrajectory.from_uniform_step( + context.robot.qpos[:, None], + env_ids=context.env_ids, + step_dt=interpolation_dt, + ), + segment_lengths={"already_satisfied": 1}, + diagnostics=diagnostics, + ) return self.failed_plan( request, context, message="Failed to resolve an articulated-part grasp pose.", ) + approach_axis = ( + grasp_xpos[:, :3, 2] + if options.approach_along_grasp_axis + else translation_axis_world + ) approach_xpos = translate_pose_world( grasp_xpos, - -translation_axis_world * options.approach_distance, + -approach_axis * options.approach_distance, ) - translation_sign = -1.0 if options.direction == "pull" else 1.0 + translation_sign = -1.0 if direction == "pull" else 1.0 translated_xpos = translate_pose_world( grasp_xpos, - translation_axis_world * (translation_sign * options.translation_distance), + translation_axis_world + * ( + translation_sign * options.translation_distance + if displacement is None + else displacement[:, None] + ), ) motion_lengths = self._motion_segment_lengths( request.motion_policy.sample_count, options.hand_interp_steps, - direction=options.direction, + direction=direction, ) approach_success, approach_arm = self._plan_pose_segment( approach_xpos, @@ -239,7 +389,7 @@ def _plan( reach_keyframes = axis_translation_keyframes( approach_xpos, grasp_xpos, - translation_axis_world, + approach_axis, n_waypoints=motion_lengths[1] - 1, ) reach_success, reach_arm = self._plan_pose_segment( @@ -268,17 +418,50 @@ def _plan( ) success = grasp_success & approach_success & reach_success & translate_success + release_arm: torch.Tensor | None = None + released_xpos = translated_xpos + if options.release_retreat_distance > 0.0: + release_axis = -grasp_xpos[:, :3, 2] + released_xpos = translate_pose_world( + translated_xpos, release_axis * options.release_retreat_distance + ) + release_keyframes = axis_translation_keyframes( + translated_xpos, + released_xpos, + release_axis, + n_waypoints=options.hand_interp_steps - 1, + ) + release_success, release_arm = self._plan_pose_segment( + release_keyframes, + translate_arm[:, -1], + control_part, + request, + options.hand_interp_steps, + interpolation_dt=interpolation_dt, + cartesian_linear=True, + ) + success = success & release_success + return_arm: torch.Tensor | None = None - if options.direction == "push": + if direction == "push": + return_axis = translation_axis_world + if options.approach_along_grasp_axis or release_arm is not None: + return_axis = approach_xpos[:, :3, 3] - released_xpos[:, :3, 3] + return_axis = torch.where( + torch.linalg.vector_norm(return_axis, dim=-1, keepdim=True) + > 1.0e-6, + return_axis, + translation_axis_world, + ) return_keyframes = axis_translation_keyframes( - translated_xpos, + released_xpos, approach_xpos, - translation_axis_world, + return_axis, n_waypoints=motion_lengths[3] - 1, ) return_success, return_arm = self._plan_pose_segment( return_keyframes, - translate_arm[:, -1], + (translate_arm if release_arm is None else release_arm)[:, -1], control_part, request, motion_lengths[3], @@ -287,8 +470,11 @@ def _plan( ) success = success & return_success + hand_preshape_qpos = torch.lerp( + hand_open_qpos, hand_grasp_qpos, options.preshape_fraction + ) hand_close = interpolate_hand_qpos( - hand_open_qpos, + hand_preshape_qpos, hand_grasp_qpos, n_waypoints=options.hand_interp_steps, ) @@ -301,7 +487,7 @@ def _plan( ("approach", approach_arm), ("reach", reach_arm), ("close", hand_close), - (options.direction, translate_arm), + (direction, translate_arm), ("open", hand_open), ] if return_arm is not None: @@ -319,9 +505,17 @@ def _plan( for arm in (approach_arm, reach_arm): stop = offset + arm.shape[1] full[:, offset:stop, arm_joint_ids] = arm - full[:, offset:stop, hand_joint_ids] = hand_open_qpos.unsqueeze(1) + full[:, offset:stop, hand_joint_ids] = hand_preshape_qpos.unsqueeze(1) offset = stop + if options.preshape_fraction > 0.0: + count = min(approach_arm.shape[1], options.hand_interp_steps) + full[:, :count, hand_joint_ids] = interpolate_hand_qpos( + context.robot.qpos[:, hand_joint_ids], + hand_preshape_qpos, + n_waypoints=count, + ) + stop = offset + hand_close.shape[1] full[:, offset:stop, arm_joint_ids] = reach_arm[:, -1].unsqueeze(1) full[:, offset:stop, hand_joint_ids] = hand_close @@ -333,7 +527,9 @@ def _plan( offset = stop stop = offset + hand_open.shape[1] - full[:, offset:stop, arm_joint_ids] = translate_arm[:, -1].unsqueeze(1) + full[:, offset:stop, arm_joint_ids] = ( + translate_arm[:, -1].unsqueeze(1) if release_arm is None else release_arm + ) full[:, offset:stop, hand_joint_ids] = hand_open offset = stop @@ -341,6 +537,10 @@ def _plan( full[:, offset:, arm_joint_ids] = return_arm full[:, offset:, hand_joint_ids] = hand_open_qpos.unsqueeze(1) + if target.joint_target is not None: + full[reached] = context.robot.qpos[reached, None] + success = (success & joint_valid) | reached + return self.build_plan( request, context, @@ -353,7 +553,10 @@ def _plan( expected_effects=StateDelta(), diagnostics=PlannerDiagnostics( backend=self.planning_services.planner_name, - metadata={"affordance_sample": {"grasp": grasp_sample.metadata}}, + metadata={ + **(diagnostics.metadata if diagnostics is not None else {}), + "affordance_sample": {"grasp": grasp_sample.metadata}, + }, ), segment_lengths=segment_lengths, # Once reach completes, contact or the commanded slide may move the @@ -363,6 +566,56 @@ def _plan( ), ) + @staticmethod + def _joint_displacement( + target: SlideJointTarget, affordance: SlideAffordance, context: PlanningContext + ) -> tuple[torch.Tensor, torch.Tensor, torch.Tensor]: + if not math.isclose( + context.scene.timestamp, context.robot.timestamp, abs_tol=1e-9, rel_tol=0 + ): + raise ValueError( + "Slide joint and robot observations must share a timestamp." + ) + limits = affordance.joint_limits + if affordance.joint_name != target.joint_name or limits is None: + raise ValueError( + "Slide joint target requires matching named affordance limits." + ) + if not limits[0] <= target.position <= limits[1]: + raise ValueError("Slide joint target is outside its declared limits.") + state = context.scene.articulation_joints.get( + (target.articulation_id, target.joint_name) + ) + if state is None: + raise ValueError( + "The exact Slide articulation joint observation is absent." + ) + position = state.position + if position.shape == (1,): + position = position.reshape(1, 1).expand(context.batch_size, 1) + if ( + position.shape != (context.batch_size, 1) + or position.device != context.robot.qpos.device + ): + raise ValueError( + "Slide joint observations must have shape (B, 1) on the planning device." + ) + position = position[:, 0] + valid = ( + torch.isfinite(position) + & (position >= limits[0] - target.tolerance) + & (position <= limits[1] + target.tolerance) + ) + if state.valid_mask is not None: + valid &= state.valid_mask + delta = (target.position - position) * target.axis_sign + reached = valid & (delta.abs() <= target.tolerance) + return ( + torch.where(valid & ~reached, delta, torch.zeros_like(delta)), + valid, + reached, + ) + @staticmethod def _require_slide_affordance( semantics: ObjectSemantics, @@ -425,5 +678,6 @@ def _plan_pose_segment( __all__ = [ "Slide", "SlideGoal", + "SlideJointTarget", "SlideOptions", ] diff --git a/embodichain/toolkits/graspkit/pg_grasp/_antipodal_backend.py b/embodichain/toolkits/graspkit/pg_grasp/_antipodal_backend.py index 20733681b..754eb808b 100644 --- a/embodichain/toolkits/graspkit/pg_grasp/_antipodal_backend.py +++ b/embodichain/toolkits/graspkit/pg_grasp/_antipodal_backend.py @@ -47,7 +47,7 @@ GRASP_ANNOTATOR_CACHE_DIR = ( Path.home() / ".cache" / "embodichain" / "grasp_annotator_cache" ) -VERSION_TAG = "v0.0.3" +_CACHE_VERSIONS = {"bounds": "v0.0.3", "centroid": "v0.0.2"} __all__: list[str] = [] @@ -108,6 +108,7 @@ def __init__( cfg=collision_cfg, ) self._sampler_cfg = sampler_cfg + self._center_mode = sampler_cfg.center_mode self._max_deviation_angle = max_deviation_angle self._approach_direction_samples = approach_direction_samples self._max_candidates = max_candidates @@ -325,8 +326,9 @@ def _get_cache_dir(self, vertices: torch.Tensor, triangles: torch.Tensor): f"largest={self._use_largest_connected_component}" ).encode("utf-8") md5_hash = hashlib.md5(vert_bytes + face_bytes + sampler_signature).hexdigest() + version = _CACHE_VERSIONS[self._center_mode] cache_path = os.path.join( - GRASP_ANNOTATOR_CACHE_DIR, f"antipodal_cache_{VERSION_TAG}_{md5_hash}.npy" + GRASP_ANNOTATOR_CACHE_DIR, f"antipodal_cache_{version}_{md5_hash}.npy" ) return cache_path @@ -527,7 +529,11 @@ def get_valid_grasp_poses( if obj_longest_axis is None: origin_points_masked = origin_points_ hit_points_masked = hit_points_ - mesh_part_center = get_pc_center_box(mesh_vert_transformed) + mesh_part_center = ( + get_pc_center_box(mesh_vert_transformed) + if self._center_mode == "bounds" + else mesh_vert_transformed.mean(dim=0) + ) else: axis = torch.as_tensor( obj_longest_axis, @@ -543,22 +549,40 @@ def get_valid_grasp_poses( raise TypeError("is_positive_part must be a bool.") axis = axis / axis_norm mesh_projection = torch.matmul(mesh_vert_transformed, axis) - mesh_center = get_pc_center_box(mesh_vert_transformed) + mesh_center = ( + get_pc_center_box(mesh_vert_transformed) + if self._center_mode == "bounds" + else mesh_vert_transformed.mean(dim=0) + ) mesh_projection_range = mesh_projection.max() - mesh_projection.min() + positive_fraction = 0.5 if self._center_mode == "bounds" else 0.65 + negative_fraction = 0.5 if self._center_mode == "bounds" else 0.35 projection_posi_threshold = ( - mesh_projection.min() + 0.5 * mesh_projection_range + mesh_projection.min() + positive_fraction * mesh_projection_range ) projection_nega_threshold = ( - mesh_projection.min() + 0.5 * mesh_projection_range + mesh_projection.min() + negative_fraction * mesh_projection_range ) pair_centers = 0.5 * (origin_points_ + hit_points_) pair_projection = torch.matmul(pair_centers, axis) if is_positive_part: - mesh_part_center = mesh_center + 0.25 * mesh_projection_range * axis part_mask = pair_projection > projection_posi_threshold + mesh_part_center = ( + mesh_center + 0.25 * mesh_projection_range * axis + if self._center_mode == "bounds" + else mesh_vert_transformed[ + mesh_projection > projection_posi_threshold + ].mean(dim=0) + ) else: - mesh_part_center = mesh_center - 0.25 * mesh_projection_range * axis part_mask = pair_projection < projection_nega_threshold + mesh_part_center = ( + mesh_center - 0.25 * mesh_projection_range * axis + if self._center_mode == "bounds" + else mesh_vert_transformed[ + mesh_projection < projection_nega_threshold + ].mean(dim=0) + ) origin_points_masked = origin_points_[part_mask] hit_points_masked = hit_points_[part_mask] return self._filter_valid_grasp_poses( @@ -591,7 +615,6 @@ def get_dual_arm_valid_grasp_poses( mesh_vert_transformed = self._apply_transform(self.vertices, object_pose) - mesh_center = get_pc_center_box(mesh_vert_transformed) mesh_projection = torch.matmul( mesh_vert_transformed, left_to_right_arm_direction ) @@ -606,11 +629,26 @@ def get_dual_arm_valid_grasp_poses( ) pair_centers = 0.5 * (origin_points_ + hit_points_) pair_projection = torch.matmul(pair_centers, left_to_right_arm_direction) - - left_center = ( - mesh_center - 0.25 * mesh_projection_range * left_to_right_arm_direction - ) + # Assign each antipodal pair exactly once using its center. Classifying + # a pair when either contact lies on a side duplicates pairs that span + # the middle gap and can send the same grasp to both arms. left_mask = pair_projection < left_threshold + right_mask = pair_projection > right_threshold + if self._center_mode == "bounds": + mesh_center = get_pc_center_box(mesh_vert_transformed) + left_center = ( + mesh_center - 0.25 * mesh_projection_range * left_to_right_arm_direction + ) + right_center = ( + mesh_center + 0.25 * mesh_projection_range * left_to_right_arm_direction + ) + else: + left_center = mesh_vert_transformed[mesh_projection < left_threshold].mean( + dim=0 + ) + right_center = mesh_vert_transformed[ + mesh_projection > right_threshold + ].mean(dim=0) is_succes_left, grasp_poses_left, open_lengths_left, total_cost_left = ( self._filter_valid_grasp_poses( hit_points_=hit_points_[left_mask], @@ -621,10 +659,6 @@ def get_dual_arm_valid_grasp_poses( visualize_collision=visualize_collision, ) ) - right_center = ( - mesh_center + 0.25 * mesh_projection_range * left_to_right_arm_direction - ) - right_mask = pair_projection > right_threshold is_succes_right, grasp_poses_right, open_lengths_right, total_cost_right = ( self._filter_valid_grasp_poses( hit_points_=hit_points_[right_mask], diff --git a/embodichain/toolkits/graspkit/pg_grasp/antipodal_sampler.py b/embodichain/toolkits/graspkit/pg_grasp/antipodal_sampler.py index d0727d40f..3ab156690 100644 --- a/embodichain/toolkits/graspkit/pg_grasp/antipodal_sampler.py +++ b/embodichain/toolkits/graspkit/pg_grasp/antipodal_sampler.py @@ -16,11 +16,14 @@ from __future__ import annotations -import torch -import torch.nn.functional as F +from typing import Literal + import numpy as np import open3d as o3d import open3d.core as o3c +import torch +import torch.nn.functional as F + from embodichain.utils import configclass from embodichain.utils import logger @@ -48,6 +51,13 @@ class AntipodalSamplerCfg: min_length: float = 0.001 """minimum gripper open width, used to filter out antipodal points that are too close to be grasped""" + center_mode: Literal["bounds", "centroid"] = "bounds" + """Mesh center used to place the enclosing ray sphere.""" + + def __post_init__(self) -> None: + if self.center_mode not in {"bounds", "centroid"}: + raise ValueError("center_mode must be 'bounds' or 'centroid'.") + class AntipodalSampler: """AntipodalSampler samples antipodal point pairs on a given mesh. It uses Open3D's raycasting functionality to find points on the mesh that are visible along the negative normal direction from uniformly sampled points on the mesh surface. The sampler can also apply a random disturbance to the ray direction to increase the diversity of sampled antipodal points. The resulting antipodal point pairs can be used for grasp generation and annotation tasks.""" @@ -78,8 +88,8 @@ def sample(self, vertices: torch.Tensor, faces: torch.Tensor) -> torch.Tensor: faces.to("cpu").numpy(), dtype=o3c.int32 ) # Sample surface points and normals by raycasting Fibonacci-distributed - # rays from outside the mesh toward its bounding-box center. Each contact - # replaces the previous uniform surface sample and keeps its face normal. + # rays toward the configured mesh center. Each contact replaces the + # previous uniform surface sample and keeps its face normal. sample_points, sample_normals = self._sample_surface_by_fibonacci_raycast( vertices, self.cfg.n_sample ) @@ -112,7 +122,7 @@ def _sample_surface_by_fibonacci_raycast( Instead of sampling points directly on the mesh surface, rays are distributed uniformly over the unit sphere using the Fibonacci spiral - and cast from a sphere enclosing the mesh toward its bounding-box center. + and cast from a sphere enclosing the mesh toward the configured center. The first contact point of each ray with the mesh is the sample, and the face normal at the contact (oriented against the ray) is its normal. @@ -147,9 +157,12 @@ def _sample_surface_by_fibonacci_raycast( [rho * torch.cos(theta), rho * torch.sin(theta), z], dim=-1 ) - # Use the bounding-box center so local mesh refinement cannot bias the rays. vertices_np = vertices.detach().to("cpu").numpy() - center = (vertices_np.min(axis=0) + vertices_np.max(axis=0)) / 2 + center = ( + (vertices_np.min(axis=0) + vertices_np.max(axis=0)) / 2 + if self.cfg.center_mode == "bounds" + else vertices_np.mean(axis=0) + ) extent = np.linalg.norm(vertices_np - center, axis=1) max_radius = float(extent.max()) if vertices_np.shape[0] > 0 else 0.0 ray_distance = 2.0 * max_radius + 1.0 # safely outside the mesh diff --git a/embodichain/toolkits/graspkit/pg_grasp/collision_checker.py b/embodichain/toolkits/graspkit/pg_grasp/collision_checker.py index 2747fa063..9bcfd5681 100644 --- a/embodichain/toolkits/graspkit/pg_grasp/collision_checker.py +++ b/embodichain/toolkits/graspkit/pg_grasp/collision_checker.py @@ -27,7 +27,7 @@ import open3d as o3d from typing import List, Tuple, Union -from dexsim.kit.meshproc import convex_decomposition_coacd +from dexsim.kit.meshproc import convex_decomposition_vhacd from embodichain.compute.geometry._warp.convex_query import ( convex_signed_distance_kernel, @@ -39,6 +39,8 @@ __all__ = ["ConvexCollisionCheckerCfg", "ConvexCollisionChecker"] +_CONVEX_DECOMPOSITION_CACHE_TAG = "vhacd_v1" + @configclass class ConvexCollisionCheckerCfg: @@ -88,7 +90,10 @@ def __init__( self.cache_path = os.path.join( CONVEX_DECOMPOSITION_CACHE_DIR, - f"{mesh_hash}_{max_decomposition_hulls}.pkl", + ( + f"{mesh_hash}_{max_decomposition_hulls}_" + f"{_CONVEX_DECOMPOSITION_CACHE_TAG}.pkl" + ), ) if not os.path.isfile(self.cache_path): @@ -301,9 +306,11 @@ def _compute_plane_equations( mesh = o3d.t.geometry.TriangleMesh() mesh.vertex.positions = o3d.core.Tensor(vertices, dtype=o3d.core.Dtype.Float32) mesh.triangle.indices = o3d.core.Tensor(faces, dtype=o3d.core.Dtype.Int32) - is_success, out_mesh_list = convex_decomposition_coacd( + is_success, out_mesh_list = convex_decomposition_vhacd( mesh, max_convex_hull_num=max_decomposition_hulls ) + if not is_success or not out_mesh_list: + raise RuntimeError("V-HACD convex decomposition failed.") convex_vert_face_list = [] for out_mesh in out_mesh_list: verts = out_mesh.vertex.positions.numpy() diff --git a/embodichain/toolkits/graspkit/pg_grasp/pose_generator.py b/embodichain/toolkits/graspkit/pg_grasp/pose_generator.py index 101475c9f..8beaa9112 100644 --- a/embodichain/toolkits/graspkit/pg_grasp/pose_generator.py +++ b/embodichain/toolkits/graspkit/pg_grasp/pose_generator.py @@ -78,6 +78,9 @@ class AntipodalGraspPoseGeneratorCfg: max_candidates: int = 50 """Maximum number of ranked candidates returned per object pose.""" + center_mode: Literal["bounds", "centroid"] = "bounds" + """Reference center used for ray sampling, partitioning, and ranking.""" + def __post_init__(self) -> None: self.sample_count = _positive_int( self.sample_count, @@ -101,6 +104,8 @@ def __post_init__(self) -> None: field_name="approach_deviation_angle", minimum=0.0, ) + if self.center_mode not in {"bounds", "centroid"}: + raise ValueError("center_mode must be 'bounds' or 'centroid'.") @configclass @@ -308,6 +313,7 @@ def _backend( max_angle=algorithm.ray_deviation_angle, max_length=model.max_opening_width, min_length=model.min_opening_width, + center_mode=algorithm.center_mode, ), collision_cfg=GripperCollisionCfg( max_open_length=model.max_opening_width, @@ -413,6 +419,29 @@ def get_valid_grasp_poses( is_positive_part: bool | torch.Tensor = True, ) -> list[tuple[torch.Tensor, torch.Tensor]]: """Return ranked candidates, optionally from one projected axis end.""" + return [ + (poses, costs) + for poses, _, costs in self.get_grasp_candidates( + mesh_vertices=mesh_vertices, + mesh_triangles=mesh_triangles, + obj_poses=obj_poses, + approach_direction=approach_direction, + obj_longest_axis=obj_longest_axis, + is_positive_part=is_positive_part, + ) + ] + + def get_grasp_candidates( + self, + *, + mesh_vertices: torch.Tensor, + mesh_triangles: torch.Tensor, + obj_poses: torch.Tensor, + approach_direction: torch.Tensor, + obj_longest_axis: torch.Tensor | None = None, + is_positive_part: bool | torch.Tensor = True, + ) -> list[tuple[torch.Tensor, torch.Tensor, torch.Tensor]]: + """Expose candidate opening widths alongside their poses and costs.""" backend = self._backend(mesh_vertices, mesh_triangles) poses = self._object_poses(obj_poses, device=backend.device) directions = self._approach_directions( @@ -457,9 +486,9 @@ def get_valid_grasp_poses( "is_positive_part must be a bool or a bool tensor with shape " f"({poses.shape[0]},)." ) - results: list[tuple[torch.Tensor, torch.Tensor]] = [] + results: list[tuple[torch.Tensor, torch.Tensor, torch.Tensor]] = [] for index, object_pose in enumerate(poses): - success, grasp_poses, _, costs = backend.get_valid_grasp_poses( + success, grasp_poses, widths, costs = backend.get_valid_grasp_poses( object_pose=object_pose, approach_direction=directions[index], obj_longest_axis=None if axes is None else axes[index], @@ -469,6 +498,11 @@ def get_valid_grasp_poses( grasp_poses = grasp_poses.unsqueeze(0) if costs.dim() == 0: costs = costs.unsqueeze(0) + widths = torch.as_tensor( + widths, device=backend.device, dtype=torch.float32 + ).reshape(-1) + if widths.shape != costs.shape or widths.shape != grasp_poses.shape[:1]: + raise ValueError("Grasp poses, opening widths and costs must align.") if not success: logger.log_warning( f"Failed to find valid grasp poses for object row {index}." @@ -479,7 +513,7 @@ def get_valid_grasp_poses( dtype=torch.float32, device=backend.device, ) - results.append((grasp_poses, costs)) + results.append((grasp_poses, widths, costs)) return results def get_best_grasp_poses( diff --git a/embodichain/toolkits/graspkit/pose_generator.py b/embodichain/toolkits/graspkit/pose_generator.py index e596b52cc..adddb1ba5 100644 --- a/embodichain/toolkits/graspkit/pose_generator.py +++ b/embodichain/toolkits/graspkit/pose_generator.py @@ -204,6 +204,27 @@ def gripper_model(self) -> ParallelJawGripperModelCfg: """Return an owned snapshot of the physical gripper model.""" return deepcopy(self._gripper_model) + def get_grasp_candidates( + self, + *, + mesh_vertices: torch.Tensor, + mesh_triangles: torch.Tensor, + obj_poses: torch.Tensor, + approach_direction: torch.Tensor, + obj_longest_axis: torch.Tensor | None = None, + is_positive_part: bool | torch.Tensor = True, + ) -> list[tuple[torch.Tensor, torch.Tensor, torch.Tensor]]: + """Return aligned poses, opening widths and costs for each object row. + + Each tuple contains tensors of shape ``(K, 4, 4)``, ``(K,)`` and + ``(K,)``. Infinite costs mark ineligible candidates. Services without + this optional metadata capability fail explicitly rather than inventing + opening widths for their existing pose-only candidates. + """ + raise NotImplementedError( + "This grasp service does not expose opening metadata." + ) + @abstractmethod def get_dual_arm_valid_grasp_poses( self, diff --git a/tests/lab/task_program/semantics/test_profiles.py b/tests/lab/task_program/semantics/test_profiles.py index 7ba7d2c2f..b4d0e7264 100644 --- a/tests/lab/task_program/semantics/test_profiles.py +++ b/tests/lab/task_program/semantics/test_profiles.py @@ -418,7 +418,7 @@ def test_engine_skills_only_exposes_visible_explicit_installed_contracts() -> No assert set(engine.skills) == expected assert "move_joints" in engine.actions - assert "move_joints" not in engine.skills + assert "move_joints" in engine.skills def test_new_skill_subclass_must_redeclare_binding_contract() -> None: diff --git a/tests/lab/task_program/test_semantic_compiler.py b/tests/lab/task_program/test_semantic_compiler.py index 9fe161a77..e4d6bc10f 100644 --- a/tests/lab/task_program/test_semantic_compiler.py +++ b/tests/lab/task_program/test_semantic_compiler.py @@ -1322,7 +1322,7 @@ def test_grounded_safe_invocation_requires_registered_dynamic_collision() -> Non assert resolved_tracking.metrics[0].tolerance == 0.125 -def test_pick_relation_lookahead_stays_late_bound_scene_dependency() -> None: +def test_pick_relation_lookahead_stays_late_bound_without_recovery_dependency() -> None: registry, _ = _scene_registry() compiler, engine = _compiler(registry) workflow = compiler.analyze( @@ -1345,7 +1345,7 @@ def test_pick_relation_lookahead_stays_late_bound_scene_dependency() -> None: assert downstream.entity_id == "table_top" request = engine.resolve(grounded.invocation) action = engine.actions["pick_up"] - assert "table_top" in action._scene_dependencies(request) + assert "table_top" not in action._scene_dependencies(request) def test_pick_replan_resolves_downstream_target_from_latest_snapshot() -> None: diff --git a/tests/sim/atomic_actions/test_actions.py b/tests/sim/atomic_actions/test_actions.py index aceb426b8..53582866c 100644 --- a/tests/sim/atomic_actions/test_actions.py +++ b/tests/sim/atomic_actions/test_actions.py @@ -75,6 +75,7 @@ OpenDoorAffordance, OpenDoorGoal, OpenDoorOptions, + PARK_COMMAND, PickUp, PickUpOptions, Place, @@ -95,6 +96,7 @@ SlideAffordance, Slide, SlideGoal, + SlideJointTarget, SlideOptions, RobotObservation, SceneEntityPose, @@ -617,6 +619,18 @@ def compute_ik( offset = 0.1 if name == "left_arm" else 0.2 return torch.ones(seed.shape[0], dtype=torch.bool), seed + offset + def compute_batch_ik( + pose: torch.Tensor, + name: str, + joint_seed: torch.Tensor, + **_: object, + ) -> tuple[torch.Tensor, torch.Tensor]: + offset = 0.1 if name == "left_arm" else 0.2 + return ( + torch.ones(pose.shape[:2], dtype=torch.bool), + joint_seed + offset, + ) + def compute_fk( qpos: torch.Tensor | None = None, name: str | None = None, @@ -628,6 +642,7 @@ def compute_fk( robot.get_qpos.side_effect = get_qpos robot.get_joint_ids.side_effect = get_joint_ids robot.compute_ik.side_effect = compute_ik + robot.compute_batch_ik.side_effect = compute_batch_ik robot.compute_fk.side_effect = compute_fk robot.get_link_pose.side_effect = get_link_pose @@ -934,7 +949,6 @@ def test_move_end_effector_returns_full_robot_timed_plan() -> None: generator = _motion_generator() action = _bind_action(generator, MoveEndEffector()) context = _context() - plan = _plan_action( action, _invocation( @@ -983,6 +997,39 @@ def test_move_joints_uses_binding_and_preserves_uncontrolled_joints() -> None: assert [target.target_id for target in plan.commands.targets] == ["arm"] +def test_move_joints_uses_park_profile_command_and_preserves_other_joints() -> None: + """The named park target stays in the embodiment's command profile.""" + generator = _motion_generator() + target = torch.full((ARM_DOF,), 0.4) + action = _bind_action( + generator, + MoveJoints(), + control_profiles={ + "arm": ControlPartCommandProfile.joint_positions(**{PARK_COMMAND: target}), + }, + ) + qpos = torch.zeros(NUM_ENVS, ROBOT_DOF) + qpos[:, ARM_DOF:] = 0.7 + context = PlanningContext( + robot=RobotObservation(0.0, qpos, torch.zeros_like(qpos)), + task=TaskState.empty(NUM_ENVS, "cpu"), + scene=SceneSnapshot.empty(), + env_ids=torch.arange(NUM_ENVS), + control_dt=CONTROL_DT, + ) + + plan = _plan_action( + action, + _invocation(action, JointPositionGoal(PARK_COMMAND), sample_count=8), + context, + ) + + assert torch.allclose(_joint_command_positions(plan, "arm")[:, -1], target) + assert [target.target_id for target in plan.commands.targets] == ["arm"] + assert plan.expected_effects.is_empty + assert MoveJoints.descriptor().agent_visible is True + + def test_pick_and_place_declare_effects_without_mutating_context() -> None: generator = _motion_generator() pick = _bind_action(generator, PickUp()) @@ -1045,15 +1092,29 @@ def test_place_holds_fully_open_before_retracting_when_configured() -> None: settle_steps = 3 invocation = ActionInvocation( skill_id=action.skill_id, - goal=PlaceGoal(torch.eye(4)), + goal=PlaceGoal(SceneEntityPose("destination")), binding=_binding(action), motion_policy=MotionPolicy(sample_count=sample_count), skill_options=PlaceOptions(release_settle_steps=settle_steps), ) - plan = _plan_action(action, invocation, _context(task)) + destination_pose = torch.eye(4).repeat(NUM_ENVS, 1, 1) + plan = _plan_action( + action, + invocation, + _context( + task, + scene=SceneSnapshot( + timestamp=0.0, + version=0, + entities={"destination": EntityState(destination_pose)}, + ), + ), + ) release = plan.segment("release") + assert plan.scene_dependencies == ("destination",) + assert plan.scene_dependency_monitor_until == {} assert plan.commands.frame_count == sample_count + settle_steps assert release.stop - release.start == 5 + settle_steps hand_positions = _joint_command_positions(plan, "hand") @@ -1252,6 +1313,161 @@ def test_move_held_object_uses_exact_projected_attachment_target() -> None: assert torch.allclose(target_states[0].xpos, held.object_to_eef) +def test_move_held_object_preserves_ordered_object_waypoints() -> None: + generator = _motion_generator() + dt = torch.full((NUM_ENVS, 4), CONTROL_DT) + dt[:, 0] = 0.0 + generator.generate = Mock( + return_value=PlanResult( + success=torch.ones(NUM_ENVS, dtype=torch.bool), + positions=torch.zeros(NUM_ENVS, 4, ARM_DOF), + dt=dt, + ) + ) + action = _bind_action(generator, MoveHeldObject()) + held = _held() + held.object_to_eef[:, 0, 3] = torch.tensor([0.03, 0.06]) + grasp_before = held.object_to_eef.clone() + task = TaskState(batch_size=NUM_ENVS, device="cpu", held_objects={"arm": held}) + targets = torch.eye(4).repeat(NUM_ENVS, 2, 1, 1) + targets[:, 0, 2, 3] = 1.3 + targets[:, 1, 2, 3] = 1.3 + targets[:, 1, :3, :3] = torch.tensor( + [[0.0, -1.0, 0.0], [1.0, 0.0, 0.0], [0.0, 0.0, 1.0]] + ) + before = targets.clone() + + plan = _plan_action( + action, _invocation(action, HeldObjectPoseGoal(targets)), _context(task) + ) + + states = generator.generate.call_args.args[0] + assert len(states) == 2 + for index, state in enumerate(states): + torch.testing.assert_close(state.xpos, targets[:, index] @ grasp_before) + assert plan.expected_effects.is_empty + torch.testing.assert_close(held.object_to_eef, grasp_before) + torch.testing.assert_close(targets, before) + assert (_joint_trajectory(plan).positions[..., ARM_DOF:] == 1).all() + + +@pytest.mark.parametrize("shape", [(NUM_ENVS, 0, 4, 4), (NUM_ENVS + 1, 2, 4, 4)]) +def test_move_held_object_rejects_invalid_waypoint_batch(shape) -> None: + generator = _motion_generator() + generator.generate = Mock() + action = _bind_action(generator, MoveHeldObject()) + task = TaskState(batch_size=NUM_ENVS, device="cpu", held_objects={"arm": _held()}) + + with pytest.raises(ValueError, match="waypoint"): + _plan_action( + action, + _invocation(action, HeldObjectPoseGoal(torch.zeros(shape))), + _context(task), + ) + generator.generate.assert_not_called() + + +def test_move_held_object_yaw_freedom_retains_successful_rows() -> None: + generator = _motion_generator() + first = PlanResult( + success=torch.tensor([True, False]), + positions=torch.full((NUM_ENVS, 3, ARM_DOF), 0.1), + dt=torch.tensor([[0.0, CONTROL_DT, CONTROL_DT]]).repeat(NUM_ENVS, 1), + ) + second = PlanResult( + success=torch.tensor([False, True]), + positions=torch.full((NUM_ENVS, 5, ARM_DOF), 0.2), + dt=torch.tensor([[0.0, CONTROL_DT, CONTROL_DT, CONTROL_DT, CONTROL_DT]]).repeat( + NUM_ENVS, 1 + ), + ) + generator.generate = Mock(side_effect=[first, second]) + action = _bind_action(generator, MoveHeldObject()) + held = _held() + held.object_to_eef[:, 0, 3] = 0.03 + held.object_to_eef[:, :3, :3] = torch.tensor( + [[0.0, 0.0, 1.0], [0.0, 1.0, 0.0], [-1.0, 0.0, 0.0]] + ) + grasp_before = held.object_to_eef.clone() + task = TaskState(batch_size=NUM_ENVS, device="cpu", held_objects={"arm": held}) + target = torch.eye(4).repeat(NUM_ENVS, 2, 1, 1) + target[:, :, :3, 3] = torch.tensor([0.1, 0.2, 1.3]) + before = target.clone() + + plan = _plan_action( + action, + _invocation(action, HeldObjectPoseGoal(target, world_yaw_free=True)), + _context(task), + ) + + assert plan.plan_success.tolist() == [True, True] + assert generator.generate.call_count == 2 + candidates = generator.generate.call_args.args[0] + torch.testing.assert_close(candidates[0].xpos, before[:, 0] @ grasp_before) + selected_object_pose = candidates[-1].xpos @ torch.linalg.inv(grasp_before) + torch.testing.assert_close(selected_object_pose[:, :3, 3], before[:, -1, :3, 3]) + torch.testing.assert_close(selected_object_pose[:, :3, 2], before[:, -1, :3, 2]) + assert not torch.allclose(selected_object_pose[:, :3, :3], before[:, -1, :3, :3]) + trajectory = _joint_trajectory(plan) + torch.testing.assert_close( + trajectory.positions[0, :, :ARM_DOF], + torch.full_like(trajectory.positions[0, :, :ARM_DOF], 0.1), + ) + torch.testing.assert_close( + trajectory.positions[1, :, :ARM_DOF], + torch.full_like(trajectory.positions[1, :, :ARM_DOF], 0.2), + ) + assert plan.expected_effects.is_empty + torch.testing.assert_close(target, before) + torch.testing.assert_close(held.object_to_eef, grasp_before) + + +@pytest.mark.parametrize("free_yaw, attempts", [(False, 1), (True, 8)]) +def test_move_held_object_yaw_search_fails_closed( + free_yaw: bool, attempts: int +) -> None: + generator = _motion_generator() + generator.generate = Mock( + return_value=PlanResult( + success=torch.zeros(NUM_ENVS, dtype=torch.bool), + positions=torch.zeros(NUM_ENVS, 3, ARM_DOF), + dt=torch.tensor([[0.0, CONTROL_DT, CONTROL_DT]]).repeat(NUM_ENVS, 1), + ) + ) + action = _bind_action(generator, MoveHeldObject()) + task = TaskState(batch_size=NUM_ENVS, device="cpu", held_objects={"arm": _held()}) + plan = _plan_action( + action, + _invocation(action, HeldObjectPoseGoal(torch.eye(4), world_yaw_free=free_yaw)), + _context(task), + ) + assert not plan.plan_success.any() + assert generator.generate.call_count == attempts + assert plan.expected_effects.is_empty + + +def test_move_held_object_yaw_search_accepts_failed_plans_without_positions() -> None: + generator = _motion_generator() + generator.generate = Mock( + return_value=PlanResult(success=torch.zeros(NUM_ENVS, dtype=torch.bool)) + ) + action = _bind_action(generator, MoveHeldObject()) + task = TaskState(batch_size=NUM_ENVS, device="cpu", held_objects={"arm": _held()}) + plan = _plan_action( + action, + _invocation(action, HeldObjectPoseGoal(torch.eye(4), world_yaw_free=True)), + _context(task), + ) + assert not plan.plan_success.any() + assert generator.generate.call_count == 8 + + +@pytest.mark.parametrize("value", [1, "true", None]) +def test_move_held_object_yaw_freedom_requires_boolean(value) -> None: + with pytest.raises(TypeError, match="boolean"): + HeldObjectPoseGoal(torch.eye(4), world_yaw_free=value) + + def test_pour_rotates_held_object_about_internal_axis_and_returns() -> None: generator = _motion_generator() solved_poses: list[torch.Tensor] = [] @@ -1274,8 +1490,8 @@ def compute_ik( semantics = ObjectSemantics( affordance=AxisAlignAffordance(internal_axis=torch.tensor([1.0, 0.0, 0.0])), geometry={}, - label="pourable-object", entity_id="pourable-object", + label="pourable-object", ) task = TaskState( batch_size=NUM_ENVS, @@ -1331,8 +1547,8 @@ def test_pour_reads_held_state_from_the_bound_logical_resource() -> None: semantics = ObjectSemantics( affordance=AxisAlignAffordance(internal_axis=torch.tensor([1.0, 0.0, 0.0])), geometry={}, - label="pourable-object", entity_id="pourable-object", + label="pourable-object", ) logical_resource = "right_manipulator" task = TaskState( @@ -1440,8 +1656,8 @@ def test_pour_requires_exclusively_held_axis_align_affordance() -> None: semantics = ObjectSemantics( affordance=AxisAlignAffordance(), geometry={}, - label="shared-pourable-object", entity_id="shared-pourable-object", + label="shared-pourable-object", ) task = TaskState( batch_size=NUM_ENVS, @@ -1697,13 +1913,27 @@ def test_pick_explicit_grasp_bypasses_sampling_and_records_grasp() -> None: [[0.0, -1.0, 0.0], [1.0, 0.0, 0.0], [0.0, 0.0, 1.0]] ) object_pose[:, 0, 3] = torch.tensor([0.03, 0.07]) - context = _context(scene=_target_scene(object_pose, timestamp=0.0, version=0)) + context = _context( + scene=SceneSnapshot( + timestamp=0.0, + version=0, + entities={ + "target": EntityState(object_pose), + "downstream": EntityState(torch.eye(4).repeat(NUM_ENVS, 1, 1)), + }, + ) + ) request = action.resolve_request( - _invocation( - action, - GraspGoal(semantics=semantics, grasp_xpos=grasp), - sample_count=20, + ActionInvocation( + skill_id=action.skill_id, + goal=GraspGoal(semantics=semantics, grasp_xpos=grasp), + binding=_binding(action), + motion_policy=MotionPolicy(sample_count=20), + skill_options=PickUpOptions( + downstream_object_target_poses=(SceneEntityPose("downstream"),), + grasp_commit_fraction=0.6, + ), ) ) plan = action.plan(request, context) @@ -1714,6 +1944,8 @@ def test_pick_explicit_grasp_bypasses_sampling_and_records_grasp() -> None: assert held is not None assert torch.allclose(held.grasp_xpos, grasp) assert torch.allclose(held.object_to_eef, torch.bmm(pose_inv(object_pose), grasp)) + # Downstream targets influence grasp selection only. They are re-grounded + # at the next Semantic Call boundary and do not invalidate active pickup. assert plan.scene_dependencies == ("target",) assert [segment.name for segment in plan.segments] == [ "approach", @@ -1722,7 +1954,7 @@ def test_pick_explicit_grasp_bypasses_sampling_and_records_grasp() -> None: ] assert plan.segment("close").stop == plan.segment("lift").start assert plan.scene_dependency_monitor_until == { - "target": plan.segment("close").start + "target": math.ceil(plan.segment("approach").stop * 0.6) } @@ -1856,14 +2088,26 @@ def compute_ik( label="axis-object", entity_id="target", ) - context = _context(scene=_target_scene(object_pose, timestamp=0.0, version=0)) + scene = _target_scene(object_pose, timestamp=0.0, version=0) + context = _context( + scene=replace( + scene, + entities={ + **scene.entities, + "grasp_target": EntityState(torch.eye(4).repeat(NUM_ENVS, 1, 1)), + }, + ) + ) original_task = context.task plan = _plan_action( action, ActionInvocation( skill_id="axis_align", - goal=AxisAlignGoal(semantics=semantics, grasp_xpos=torch.eye(4)), + goal=AxisAlignGoal( + semantics=semantics, + grasp_xpos=SceneEntityPose("grasp_target"), + ), binding=_binding(action), motion_policy=MotionPolicy(sample_count=20), skill_options=AxisAlignOptions( @@ -1893,7 +2137,8 @@ def compute_ik( torch.testing.assert_close(held.object_to_eef, object_pose) torch.testing.assert_close(held.grasp_xpos, solved_poses[-1]) assert context.task is original_task - assert plan.scene_dependencies == ("target",) + assert plan.scene_dependencies == ("grasp_target", "target") + assert plan.scene_dependency_monitor_until == {"target": plan.segments[0].stop} final_object_rotation = solved_poses[-1][:, :3, :3] final_world_axis = torch.matmul( final_object_rotation, @@ -2015,8 +2260,8 @@ def test_axis_align_validates_goal_and_binding_contract() -> None: semantics = ObjectSemantics( affordance=AxisAlignAffordance(), geometry={}, - label="axis-object", entity_id="axis-object", + label="axis-object", ) with pytest.raises(TypeError, match="expects goal AxisAlignGoal"): @@ -2451,6 +2696,12 @@ def test_pick_uses_selected_control_part_for_state_and_commands() -> None: "alternate_arm", "alternate_hand", } + assert plan.tracking is not None + assert all( + {setpoint.endpoint_key for setpoint in frame.setpoints} + == {("primary", "motion")} + for frame in plan.tracking.frames + ) def test_press_closes_hand_without_changing_projected_attachment() -> None: @@ -2750,10 +3001,14 @@ def test_twist_session_replans_when_scene_target_moves() -> None: ), ), ) +@pytest.mark.parametrize("calibrated", [False, True]) +@pytest.mark.parametrize("release_failure", [False, True]) def test_slide_plans_expected_segments( direction: Literal["pull", "push"], expected_segments: list[str], translation_sign: float, + calibrated: bool, + release_failure: bool, ) -> None: vertices = torch.tensor( [ @@ -2793,7 +3048,27 @@ def sample_grasp( hand_interp_steps=3, approach_distance=0.1, translation_distance=0.15, + **( + { + "approach_along_grasp_axis": True, + "preshape_fraction": 0.85, + "release_retreat_distance": 0.04, + } + if calibrated + else {} + ), ) + if calibrated and release_failure: + original_plan = action._plan_pose_segment + + def fail_release(*args, **kwargs): + success, positions = original_plan(*args, **kwargs) + if args[4] == options.hand_interp_steps: + success = success.clone() + success[1] = False + return success, positions + + action._plan_pose_segment = fail_release plan = _plan_action( action, @@ -2808,13 +3083,16 @@ def sample_grasp( ) trajectory = _joint_trajectory(plan) - assert plan.plan_success.tolist() == [True, True] + assert plan.plan_success.tolist() == [True, not (calibrated and release_failure)] assert plan.scene_dependencies == ("target",) assert plan.scene_dependency_end_segment == "reach" assert trajectory.positions.shape == (NUM_ENVS, 24, ROBOT_DOF) assert [segment.name for segment in plan.segments] == expected_segments assert torch.all( - trajectory.positions[:, plan.segment("close").stop - 1, ARM_DOF:] == 1.0 + trajectory.positions[ + plan.plan_success, plan.segment("close").stop - 1, ARM_DOF: + ] + == 1.0 ) assert torch.all( trajectory.positions[:, plan.segment("open").stop - 1, ARM_DOF:] == 0.0 @@ -2829,6 +3107,7 @@ def sample_grasp( call.kwargs["pose"] for call in generator.robot.compute_ik.call_args_list ] expected_axis = torch.tensor([0.0, -1.0, 0.0]) + approach_axis = torch.tensor([0.0, 0.0, 1.0]) if calibrated else expected_axis motion_lengths = Slide._motion_segment_lengths( 24, options.hand_interp_steps, @@ -2836,7 +3115,7 @@ def sample_grasp( ) assert torch.allclose( planned_targets[0][:, :3, 3], - -expected_axis.expand(NUM_ENVS, -1) * options.approach_distance, + -approach_axis.expand(NUM_ENVS, -1) * options.approach_distance, ) reach_stop = 1 + motion_lengths[1] - 1 assert torch.allclose( @@ -2858,75 +3137,327 @@ def sample_grasp( - (translated_targets * expected_axis).sum(dim=-1, keepdim=True) * expected_axis ) assert torch.allclose(orthogonal, torch.zeros_like(orthogonal), atol=1.0e-6) + if calibrated: + eligible = plan.plan_success + for segment in ("approach", "reach"): + assert torch.allclose( + trajectory.positions[ + eligible, plan.segment(segment).stop - 1, ARM_DOF: + ], + torch.full_like(trajectory.positions[eligible, 0, ARM_DOF:], 0.85), + ) + assert torch.allclose( + trajectory.positions[eligible, 0, ARM_DOF:], + _context().robot.qpos[eligible, ARM_DOF:], + ) + release_end = translated_targets[:, -1].clone() + release_end[:, 2] -= options.release_retreat_distance + assert torch.allclose( + planned_targets[translate_stop + options.hand_interp_steps - 2][:, :3, 3], + release_end, + ) if direction == "push": assert torch.allclose( planned_targets[-1][:, :3, 3], - -expected_axis.expand(NUM_ENVS, -1) * options.approach_distance, + -approach_axis.expand(NUM_ENVS, -1) * options.approach_distance, ) -def test_slide_holds_failed_environment() -> None: +@pytest.mark.parametrize( + "kwargs", + [ + {"preshape_fraction": -0.1}, + {"preshape_fraction": 1.1}, + {"preshape_fraction": float("nan")}, + {"preshape_fraction": True}, + {"release_retreat_distance": -0.01}, + {"release_retreat_distance": float("inf")}, + {"release_retreat_distance": True}, + {"release_retreat_distance": 0.04, "hand_interp_steps": 1}, + {"approach_along_grasp_axis": "yes"}, + ], +) +def test_slide_rejects_invalid_clearance_options(kwargs: dict) -> None: + with pytest.raises((ValueError, TypeError)): + SlideOptions(**kwargs) + + +@pytest.mark.parametrize( + "target_position,positions,direction", + [(0.2, [0.0, 0.1], "push"), (0.0, [0.2, 0.1], "pull"), (0.2, [0.2, 0.1], "push")], +) +@pytest.mark.parametrize("axis_sign", [1, -1]) +def test_slide_joint_target_uses_fresh_row_local_motion( + target_position: float, positions: list[float], direction: str, axis_sign: int +) -> None: + if axis_sign == -1: + direction = "pull" if direction == "push" else "push" affordance = SlideAffordance( mesh_vertices=torch.zeros(3, 3), mesh_triangles=torch.tensor([[0, 1, 2]]), - translation_axis=torch.tensor([0.0, -1.0, 0.0]), - ) - semantics = ObjectSemantics( - affordance=affordance, - geometry={}, - label="drawer_handle", - entity_id="drawer_handle", + translation_axis=torch.tensor([0.0, 1.0, 0.0]), + joint_name="drawer_joint", + joint_limits=(0.0, 0.3), ) generator = _motion_generator() - - def successful_ik( - pose: torch.Tensor | None = None, - name: str | None = None, - joint_seed: torch.Tensor | None = None, - **_: object, - ) -> tuple[torch.Tensor, torch.Tensor]: - assert joint_seed is not None - return torch.ones(NUM_ENVS, dtype=torch.bool), torch.ones_like(joint_seed) - - generator.robot.compute_ik.side_effect = successful_ik action = _bind_action(generator, Slide()) - _GRASP_GENERATORS[id(action)].get_valid_grasp_poses = Mock( - return_value=[ - (torch.eye(4).unsqueeze(0), torch.zeros(1)), - (torch.empty(0, 4, 4), torch.empty(0)), - ] - ) - context = _context() - - plan = _plan_action( - action, - ActionInvocation( - skill_id="slide", - goal=SlideGoal(semantics, torch.eye(4)), - binding=_binding(action), - motion_policy=MotionPolicy(sample_count=18), - skill_options=SlideOptions(hand_interp_steps=3), + goal = SlideGoal( + ObjectSemantics(affordance=affordance, geometry={}, entity_id="target"), + SceneEntityPose("target"), + joint_target=SlideJointTarget( + "drawer", "drawer_joint", target_position, axis_sign=axis_sign ), - context, ) - - assert plan.plan_success.tolist() == [True, False] - trajectory = _joint_trajectory(plan) - assert not torch.allclose(trajectory.positions[0], context.robot.qpos[0]) + scene = replace( + _target_scene(torch.eye(4).repeat(NUM_ENVS, 1, 1), timestamp=0.0, version=0), + articulation_joints={ + ("drawer", "drawer_joint"): ObservedArticulationJointState( + torch.tensor(positions)[:, None] + ) + }, + ) + options = SlideOptions( + direction="pull", translation_distance=0.15, hand_interp_steps=3 + ) + invocation = ActionInvocation( + skill_id="slide", + goal=goal, + binding=_binding(action), + motion_policy=MotionPolicy(sample_count=24), + skill_options=options, + ) + plan = _plan_action(action, invocation, _context(scene=scene)) + assert plan.plan_success.tolist() == [True, True] + assert direction in [s.name for s in plan.segments] + assert plan.diagnostics.metadata["joint_target"]["position"] == target_position + assert ( + plan.diagnostics.metadata["affordance_sample"]["grasp"]["key"] == "slide:grasp" + ) + lengths = Slide._motion_segment_lengths(24, 3, direction=direction) + poses = [c.kwargs["pose"] for c in generator.robot.compute_ik.call_args_list] + endpoint = poses[lengths[1] + lengths[2] - 2] + active = torch.tensor(positions) != target_position assert torch.allclose( - trajectory.positions[1], - context.robot.qpos[1].unsqueeze(0).expand(18, -1), + endpoint[active, 1, 3], + (target_position - torch.tensor(positions)[active]) * axis_sign, + atol=1e-6, + ) + reached = ~active + assert plan.diagnostics.metadata["joint_target"] == { + "articulation_id": "drawer", + "joint_name": "drawer_joint", + "position": target_position, + "already_satisfied": reached.tolist(), + } + assert "grasp" in plan.diagnostics.metadata["affordance_sample"] + assert torch.equal( + plan.joint_trajectory.positions[reached], + torch.zeros_like(plan.joint_trajectory.positions[reached]), ) + assert options.translation_distance == 0.15 and options.direction == "pull" + # A fresh planning snapshot changes remaining motion without mutating the goal. + advanced = replace( + scene, + articulation_joints={ + ("drawer", "drawer_joint"): ObservedArticulationJointState( + torch.full((NUM_ENVS, 1), target_position) + ) + }, + ) + held = _plan_action(action, invocation, _context(scene=advanced)) + assert held.plan_success.all() + assert [s.name for s in held.segments] == ["already_satisfied"] + assert held.diagnostics.metadata["joint_target"]["already_satisfied"] == [ + True, + True, + ] + assert "affordance_sample" not in held.diagnostics.metadata -def test_slide_fk_path_remains_on_translation_axis() -> None: - generator = _motion_generator() - - def position_ik( - pose: torch.Tensor, - name: str, - joint_seed: torch.Tensor, - ) -> tuple[torch.Tensor, torch.Tensor]: +@pytest.mark.parametrize( + "case", ["missing", "wrong_joint", "outside", "stale", "mixed"] +) +def test_slide_joint_target_rejects_incompatible_binding_before_ik(case: str) -> None: + affordance = SlideAffordance( + mesh_vertices=torch.zeros(3, 3), + mesh_triangles=torch.tensor([[0, 1, 2]]), + joint_name="joint", + joint_limits=(0.0, 0.3), + ) + target = SlideJointTarget( + "drawer", + "other" if case == "wrong_joint" else "joint", + 0.4 if case == "outside" else 0.1, + axis_sign=1, + ) + scene = replace( + SceneSnapshot.empty(), + timestamp=1.0 if case == "stale" else 0.0, + articulation_joints=( + {} + if case == "missing" + else { + ("drawer", "joint"): ObservedArticulationJointState( + torch.tensor([[0.0], [0.2 if case == "mixed" else 0.0]]) + ) + } + ), + ) + generator = _motion_generator() + action = _bind_action(generator, Slide()) + invocation = ActionInvocation( + skill_id="slide", + goal=SlideGoal( + ObjectSemantics(affordance=affordance, geometry={}, entity_id="target"), + torch.eye(4), + joint_target=target, + ), + binding=_binding(action), + ) + with pytest.raises(ValueError): + _plan_action(action, invocation, _context(scene=scene)) + generator.robot.compute_ik.assert_not_called() + + +def test_slide_satisfied_joint_target_still_resolves_scene_pose() -> None: + affordance = SlideAffordance( + mesh_vertices=torch.zeros(3, 3), + mesh_triangles=torch.tensor([[0, 1, 2]]), + joint_name="joint", + joint_limits=(0.0, 0.3), + ) + generator = _motion_generator() + action = _bind_action(generator, Slide()) + invocation = ActionInvocation( + skill_id="slide", + goal=SlideGoal( + ObjectSemantics(affordance=affordance, geometry={}, entity_id="target"), + SceneEntityPose("missing"), + joint_target=SlideJointTarget("drawer", "joint", 0.2, axis_sign=1), + ), + binding=_binding(action), + ) + scene = replace( + SceneSnapshot.empty(), + articulation_joints={ + ("drawer", "joint"): ObservedArticulationJointState( + torch.full((NUM_ENVS, 1), 0.2) + ) + }, + ) + with pytest.raises(KeyError, match="missing"): + _plan_action(action, invocation, _context(scene=scene)) + generator.robot.compute_ik.assert_not_called() + + +@pytest.mark.parametrize("case", ["invalid_observation", "sampler_failure"]) +def test_slide_joint_target_preserves_reached_rows_when_other_rows_fail( + case: str, +) -> None: + affordance = SlideAffordance( + mesh_vertices=torch.zeros(3, 3), + mesh_triangles=torch.tensor([[0, 1, 2]]), + joint_name="joint", + joint_limits=(0.0, 0.3), + ) + generator = _motion_generator() + action = _bind_action(generator, Slide()) + _GRASP_GENERATORS[id(action)].get_valid_grasp_poses = Mock( + return_value=[(torch.empty(0, 4, 4), torch.empty(0)) for _ in range(NUM_ENVS)] + ) + scene = replace( + SceneSnapshot.empty(), + articulation_joints={ + ("drawer", "joint"): ObservedArticulationJointState( + torch.tensor([[0.2], [0.0]]), + valid_mask=torch.tensor([True, case != "invalid_observation"]), + ) + }, + ) + invocation = ActionInvocation( + skill_id="slide", + goal=SlideGoal( + ObjectSemantics(affordance=affordance, geometry={}, entity_id="target"), + torch.eye(4), + joint_target=SlideJointTarget("drawer", "joint", 0.2, axis_sign=1), + ), + binding=_binding(action), + ) + plan = _plan_action(action, invocation, _context(scene=scene)) + assert plan.plan_success.tolist() == [True, False] + assert plan.diagnostics.metadata["joint_target"]["already_satisfied"] == [ + True, + False, + ] + assert torch.equal( + plan.joint_trajectory.positions, + torch.zeros_like(plan.joint_trajectory.positions), + ) + generator.robot.compute_ik.assert_not_called() + + +def test_slide_holds_failed_environment() -> None: + affordance = SlideAffordance( + mesh_vertices=torch.zeros(3, 3), + mesh_triangles=torch.tensor([[0, 1, 2]]), + translation_axis=torch.tensor([0.0, -1.0, 0.0]), + ) + semantics = ObjectSemantics( + affordance=affordance, + geometry={}, + label="drawer_handle", + entity_id="drawer_handle", + ) + generator = _motion_generator() + + def successful_ik( + pose: torch.Tensor | None = None, + name: str | None = None, + joint_seed: torch.Tensor | None = None, + **_: object, + ) -> tuple[torch.Tensor, torch.Tensor]: + assert joint_seed is not None + return torch.ones(NUM_ENVS, dtype=torch.bool), torch.ones_like(joint_seed) + + generator.robot.compute_ik.side_effect = successful_ik + action = _bind_action(generator, Slide()) + _GRASP_GENERATORS[id(action)].get_valid_grasp_poses = Mock( + return_value=[ + (torch.eye(4).unsqueeze(0), torch.zeros(1)), + (torch.empty(0, 4, 4), torch.empty(0)), + ] + ) + context = _context() + + plan = _plan_action( + action, + ActionInvocation( + skill_id="slide", + goal=SlideGoal(semantics, torch.eye(4)), + binding=_binding(action), + motion_policy=MotionPolicy(sample_count=18), + skill_options=SlideOptions(hand_interp_steps=3), + ), + context, + ) + + assert plan.plan_success.tolist() == [True, False] + trajectory = _joint_trajectory(plan) + assert not torch.allclose(trajectory.positions[0], context.robot.qpos[0]) + assert torch.allclose( + trajectory.positions[1], + context.robot.qpos[1].unsqueeze(0).expand(18, -1), + ) + + +def test_slide_fk_path_remains_on_translation_axis() -> None: + generator = _motion_generator() + + def position_ik( + pose: torch.Tensor, + name: str, + joint_seed: torch.Tensor, + ) -> tuple[torch.Tensor, torch.Tensor]: qpos = joint_seed.clone() qpos[:, :3] = pose[:, :3, 3] return torch.ones(NUM_ENVS, dtype=torch.bool), qpos @@ -4560,8 +5091,273 @@ def plan_from_start( assert context.task is original_task assert plan.scene_dependencies == ("handover_object",) assert plan.scene_dependency_monitor_until == { - "handover_object": plan.segment("pickup_close").stop + "handover_object": plan.segment("pickup_close").start + } + + +def test_handover_can_end_with_receiving_resource_holding_object( + monkeypatch: pytest.MonkeyPatch, +) -> None: + """Transfer-only mode stops after source release and publishes held state.""" + action = _bind_action(_dual_motion_generator(), HandOver()) + semantics, _ = _handover_semantics() + object_pose = torch.eye(4).repeat(NUM_ENVS, 1, 1) + object_pose[:, :3, 3] = torch.tensor([-0.8, 0.1, 0.5]) + + action._resolve_grasp = Mock( + side_effect=lambda _affordance, sampled_pose, *_args, **kwargs: AffordanceSample( + success=torch.ones(NUM_ENVS, dtype=torch.bool), + poses=sampled_pose.clone(), + metadata={"key": kwargs["sample_key"]}, + ) + ) + + def plan_from_start( + motion_generator: MotionGenerator, + control_part: str, + start_qpos: torch.Tensor, + target_poses: torch.Tensor, + n_waypoints: int, + motion_policy: MotionPolicy, + interpolation_dt: float | None, + ) -> tuple[torch.Tensor, torch.Tensor]: + del ( + motion_generator, + control_part, + target_poses, + motion_policy, + interpolation_dt, + ) + trajectory = start_qpos.unsqueeze(1).repeat(1, n_waypoints, 1) + return torch.ones(NUM_ENVS, dtype=torch.bool), trajectory + + monkeypatch.setattr( + "embodichain.lab.sim.atomic_actions.primitives.hand_over." + "plan_named_arm_trajectory", + plan_from_start, + ) + final_pose = torch.eye(4) + final_pose[2, 3] = 2.0 + invocation = ActionInvocation( + skill_id="hand_over", + goal=HandOverGoal(semantics, target_pose=final_pose), + binding=_dual_binding(action, "source", "destination"), + motion_policy=MotionPolicy(sample_count=18), + skill_options=HandOverOptions( + hand_interp_steps=2, + release_at_target=False, + arm_selection="bound", + ), + ) + context = _handover_context(object_pose) + + plan = _plan_action(action, invocation, context) + + assert plan.plan_success.tolist() == [True, True] + assert [segment.name for segment in plan.segments] == [ + "pickup_approach", + "pickup_close", + "pickup_transport", + "receive_approach", + "receive_close", + "handover_release", + "source_retreat", + ] + projected = plan.expected_effects.apply( + context.task, + torch.ones(NUM_ENVS, dtype=torch.bool), + ) + assert plan.expected_effects.held_object_updates["left_arm"] is None + assert projected.get_held_object("left_arm") is None + received = projected.get_held_object("right_arm") + assert isinstance(received, HeldObjectState) + assert received.env_mask is not None and received.env_mask.tolist() == [True, True] + assert all( + candidate.env_mask is not None and candidate.env_mask.tolist() == [True, True] + for candidate in plan.effect_candidates.held_object_updates.values() + if isinstance(candidate, HeldObjectState) + ) + assert plan.scene_dependency_monitor_until == { + "handover_object": plan.segment("pickup_close").start } + trajectory = _joint_trajectory(plan) + assert torch.all(trajectory.positions[:, -1, DUAL_ARM_DOF : DUAL_ARM_DOF + 2] == 0) + assert torch.all(trajectory.positions[:, -1, DUAL_ARM_DOF + 2 :] == 1) + + +def test_handover_existing_hold_uses_root_midpoint_and_absolute_height( + monkeypatch: pytest.MonkeyPatch, +) -> None: + """The receiver enters diagonally from its embodiment side.""" + action = _bind_action(_dual_motion_generator(), HandOver()) + semantics, _ = _handover_semantics() + held = _held(semantics, env_mask=torch.ones(NUM_ENVS, dtype=torch.bool)) + task = TaskState( + batch_size=NUM_ENVS, + device="cpu", + held_objects={"left_arm": held}, + ) + action._resolve_grasp = Mock( + side_effect=lambda _affordance, sampled_pose, *_args, **kwargs: AffordanceSample( + success=torch.ones(NUM_ENVS, dtype=torch.bool), + poses=sampled_pose.clone(), + metadata={"key": kwargs["sample_key"]}, + ) + ) + planned_targets: list[torch.Tensor] = [] + + def plan_from_start( + motion_generator: MotionGenerator, + control_part: str, + start_qpos: torch.Tensor, + target_poses: torch.Tensor, + n_waypoints: int, + motion_policy: MotionPolicy, + interpolation_dt: float | None, + ) -> tuple[torch.Tensor, torch.Tensor]: + del motion_generator, control_part, motion_policy, interpolation_dt + planned_targets.append(target_poses.clone()) + trajectory = start_qpos.unsqueeze(1).repeat(1, n_waypoints, 1) + return torch.ones(NUM_ENVS, dtype=torch.bool), trajectory + + monkeypatch.setattr( + "embodichain.lab.sim.atomic_actions.primitives.hand_over." + "plan_named_arm_trajectory", + plan_from_start, + ) + exchange = torch.eye(4) + exchange[0, 3] = 0.6 + exchange[1, 3] = 0.4 + exchange[2, 3] = 0.05 + + context = _handover_context(torch.eye(4).repeat(NUM_ENVS, 1, 1), task) + source_hand_qpos = torch.tensor([[0.35, 0.45], [0.55, 0.65]]) + qpos = context.robot.qpos.clone() + qpos[:, DUAL_ARM_DOF : DUAL_ARM_DOF + HAND_DOF] = source_hand_qpos + context = replace( + context, + robot=replace(context.robot, qpos=qpos), + scene=replace( + context.scene, + entities={ + **context.scene.entities, + "handover_target": EntityState(exchange), + }, + ), + ) + + plan = _plan_action( + action, + ActionInvocation( + skill_id="hand_over", + goal=HandOverGoal( + semantics, + target_pose=SceneEntityPose("handover_target"), + ), + binding=_dual_binding(action, "source", "destination"), + motion_policy=MotionPolicy(sample_count=60), + skill_options=HandOverOptions( + release_at_target=False, + receive_pick_object_part="center", + ), + ), + context, + ) + + assert plan.plan_success.tolist() == [True, True] + grasp_call = action._resolve_grasp.call_args + expected = torch.tensor([-math.sin(math.pi / 3), 0.0, -0.5]).expand(NUM_ENVS, -1) + torch.testing.assert_close(grasp_call.args[2], expected) + assert grasp_call.kwargs["obj_longest_axis"] is None + torch.testing.assert_close( + grasp_call.kwargs["center_axis"], + torch.tensor([[0.0, 0.0, 1.0]]).expand(NUM_ENVS, -1), + ) + # Root geometry owns the shared-workspace coordinate. The provider's x/y + # values are final-delivery hints and cannot bias a transfer-only route. + torch.testing.assert_close( + planned_targets[0][:, 0, :2, 3], + torch.zeros(NUM_ENVS, 2), + ) + # The configured provider still owns the absolute safe exchange height; + # existing-hold mode must not add lift_height once more. + torch.testing.assert_close( + planned_targets[0][:, 0, 2, 3], + torch.full((NUM_ENVS,), 0.05), + ) + assert [segment.name for segment in plan.segments] == [ + "transfer", + "receive_approach", + "receive_close", + "receive_hold", + "handover_release", + "source_retreat", + ] + assert plan.scene_dependencies == ("handover_object", "handover_target") + assert plan.scene_dependency_monitor_until == {"handover_object": 0} + trajectory = _joint_trajectory(plan).positions + transfer = plan.segment("transfer") + release = plan.segment("handover_release") + torch.testing.assert_close( + trajectory[ + :, transfer.start : release.start, DUAL_ARM_DOF : DUAL_ARM_DOF + HAND_DOF + ], + source_hand_qpos[:, None].expand(-1, release.start - transfer.start, -1), + ) + torch.testing.assert_close( + trajectory[:, release.start, DUAL_ARM_DOF : DUAL_ARM_DOF + HAND_DOF], + source_hand_qpos, + ) + + +@pytest.mark.parametrize("sampling_enabled", [False, True]) +def test_handover_center_grasp_rejects_lower_cost_outer_candidates( + sampling_enabled: bool, +) -> None: + """Center mode selects the object's middle third, not either end.""" + action = _bind_action(_dual_motion_generator(), HandOver()) + vertices = torch.tensor( + [ + [-0.1, -0.1, -1.0], + [0.1, -0.1, -1.0], + [0.1, 0.1, 1.0], + [-0.1, 0.1, 1.0], + ], + dtype=torch.float32, + ) + affordance = AntipodalAffordance( + mesh_vertices=vertices, + mesh_triangles=torch.tensor([[0, 1, 2], [0, 2, 3]]), + ) + candidates = torch.eye(4).repeat(3, 1, 1) + candidates[:, 2, 3] = torch.tensor([0.8, 0.0, -0.8]) + costs = torch.tensor([0.0, 1.0, 0.5]) + _GRASP_GENERATORS[id(action)].get_valid_grasp_poses = Mock( + return_value=[(candidates, costs) for _ in range(NUM_ENVS)] + ) + + context = _handover_context(torch.eye(4).repeat(NUM_ENVS, 1, 1)) + if sampling_enabled: + context = replace( + context, + affordance_sampling=AffordanceSamplingContext(count=NUM_ENVS, seed=7), + ) + sample = action._resolve_grasp( + affordance, + torch.eye(4).repeat(NUM_ENVS, 1, 1), + torch.tensor([[1.0, 0.0, -1.0]]).expand(NUM_ENVS, -1), + "right_hand", + obj_longest_axis=None, + is_positive_part=torch.ones(NUM_ENVS, dtype=torch.bool), + center_axis=torch.tensor([[0.0, 0.0, 1.0]]).expand(NUM_ENVS, -1), + context=context, + sample_key="handover:center_grasp", + ) + + assert sample.success.tolist() == [True, True] + torch.testing.assert_close(sample.poses[:, 2, 3], torch.zeros(NUM_ENVS)) + assert sample.metadata["candidate_ids"] == [1, 1] + assert sample.metadata["valid_candidate_counts"] == [1, 1] def test_handover_horizontal_mode_uses_downward_opposite_end_grasps() -> None: @@ -4968,6 +5764,69 @@ def test_handover_requires_antipodal_affordance_and_valid_options() -> None: HandOverOptions(lift_height=float("nan")) with pytest.raises(ValueError, match="hand_interp_steps"): HandOverOptions(hand_interp_steps=0) + with pytest.raises(ValueError, match="retreat_distance"): + HandOverOptions(retreat_distance=float("nan")) + + +def test_handover_source_retreat_retraces_grasp_before_lifting() -> None: + source = torch.eye(4).repeat(NUM_ENVS, 1, 1) + source[:, 1, 3] = -0.06 + source[:, 2, 3] = 0.90 + source[:, :3, :3] = torch.diag(torch.tensor([1.0, -1.0, -1.0])) + destination = torch.eye(4).repeat(NUM_ENVS, 1, 1) + destination[:, 2, 3] = 0.88 + + waypoints = HandOver._source_retreat_waypoints( + source, + destination, + source_fallback=source, + destination_fallback=destination, + retreat_distance=0.12, + lift_height=0.08, + ) + + assert waypoints.shape == (NUM_ENVS, 5, 4, 4) + expected_y = torch.full((5,), -0.06) + expected_z = torch.tensor([0.94, 0.98, 1.02, 1.06, 1.10]) + torch.testing.assert_close( + waypoints[:, :, 1, 3], + expected_y.expand(NUM_ENVS, -1), + ) + torch.testing.assert_close( + waypoints[:, :, 2, 3], + expected_z.expand(NUM_ENVS, -1), + ) + torch.testing.assert_close( + waypoints[:, :, :3, :3], + source[:, None, :3, :3].expand(-1, 5, -1, -1), + ) + + +def test_handover_source_retreat_does_not_sweep_toward_receiver() -> None: + source_exchange = torch.eye(4).repeat(NUM_ENVS, 1, 1) + source_exchange[:, 1, 3] = -0.06 + source_exchange[:, :3, :3] = torch.diag(torch.tensor([1.0, -1.0, -1.0])) + destination_grasp = torch.eye(4).repeat(NUM_ENVS, 1, 1) + source_start = torch.eye(4).repeat(NUM_ENVS, 1, 1) + source_start[:, 1, 3] = -0.5 + destination_start = torch.eye(4).repeat(NUM_ENVS, 1, 1) + destination_start[:, 1, 3] = 0.5 + + waypoints = HandOver._source_retreat_waypoints( + source_exchange, + destination_grasp, + source_fallback=source_start, + destination_fallback=destination_start, + retreat_distance=0.12, + lift_height=0.08, + ) + + torch.testing.assert_close( + waypoints[:, -1, :2, 3], + torch.tensor([[0.0, -0.06]]).expand(NUM_ENVS, -1), + ) + torch.testing.assert_close(waypoints[:, -1, 2, 3], torch.full((NUM_ENVS,), 0.20)) + torch.testing.assert_close(waypoints[:, -1, :3, :3], source_exchange[:, :3, :3]) def test_handover_rejects_link_scoped_grasp_mesh() -> None: @@ -4991,6 +5850,23 @@ def test_handover_rejects_link_scoped_grasp_mesh() -> None: ) +@pytest.mark.parametrize( + "kwargs", + [ + {"retreat_distance": float("nan")}, + {"retreat_distance": True}, + {"retreat_steps": 2.5}, + {"release_steps": float("nan")}, + {"middle_empty_ratio": 1.0}, + ], +) +def test_coordinated_pick_rejects_nonfinite_or_nonintegral_options( + kwargs: dict[str, object], +) -> None: + with pytest.raises((TypeError, ValueError)): + CoordinatedPickmentOptions(**kwargs) + + @pytest.mark.parametrize( ("hold_steps", "expected_segments"), ( @@ -5078,9 +5954,14 @@ def test_coordinated_pick_rejects_link_scoped_grasp_mesh() -> None: action._resolve_dual_arm_grasp_poses( semantics, torch.eye(4).repeat(NUM_ENVS, 1, 1), + torch.eye(4).repeat(NUM_ENVS, 1, 1), CoordinatedPickmentOptions(), "left_hand", "right_hand", + torch.zeros(NUM_ENVS, ARM_DOF), + torch.zeros(NUM_ENVS, ARM_DOF), + "left_arm", + "right_arm", ) @@ -5129,6 +6010,9 @@ def test_coordinated_pick_implicit_initial_pose_uses_scene_snapshot() -> None: ].get_dual_arm_valid_grasp_poses.call_args.kwargs["obj_poses"] assert torch.equal(sampled_pose, object_pose) assert plan.scene_dependencies == ("target",) + assert plan.scene_dependency_monitor_until == { + "target": math.ceil(plan.segment("approach").stop / 2) + } left_held = projected.get_held_object("left_arm") right_held = projected.get_held_object("right_arm") assert left_held is not None and right_held is not None @@ -5137,6 +6021,309 @@ def test_coordinated_pick_implicit_initial_pose_uses_scene_snapshot() -> None: assert torch.allclose(right_held.object_to_eef, pose_inv(object_pose)) +def test_coordinated_pick_selects_reachable_candidate_over_lower_cost() -> None: + generator = _dual_motion_generator() + original_batch_ik = generator.robot.compute_batch_ik.side_effect + + def reject_distant_candidate( + pose: torch.Tensor, + name: str, + joint_seed: torch.Tensor, + **kwargs: object, + ) -> tuple[torch.Tensor, torch.Tensor]: + success, qpos = original_batch_ik( + pose=pose, + name=name, + joint_seed=joint_seed, + **kwargs, + ) + return success & (pose[..., 0, 3].abs() < 1.0), qpos + + generator.robot.compute_batch_ik.side_effect = reject_distant_candidate + action = _bind_action( + generator, + CoordinatedPickment( + default_options=CoordinatedPickmentOptions( + hand_interp_steps=4, + hold_steps=2, + object_motion_keyframes=3, + ), + ), + ) + distant = torch.eye(4, dtype=torch.float32) + distant[0, 3] = 5.0 + reachable = torch.eye(4, dtype=torch.float32) + candidates = torch.stack((distant, reachable)) + arm_result = { + "is_success": True, + "grasp_poses": candidates, + "open_lengths": torch.zeros(2), + "total_cost": torch.tensor([0.0, 1.0]), + } + _GRASP_GENERATORS[id(action)].get_dual_arm_valid_grasp_poses = Mock( + return_value=[ + {"left": arm_result, "right": arm_result} for _ in range(NUM_ENVS) + ] + ) + invocation = ActionInvocation( + skill_id="coordinated_pickment", + goal=CoordinatedPickGoal( + semantics=ObjectSemantics( + affordance=AntipodalAffordance(), + geometry={}, + label="tray", + entity_id="tray", + ), + object_target_pose=torch.eye(4), + object_initial_pose=torch.eye(4), + ), + binding=_dual_binding(action, "left", "right"), + motion_policy=MotionPolicy(sample_count=30), + ) + context = _dual_context() + + plan = _plan_action(action, invocation, context) + projected = plan.expected_effects.apply(context.task, plan.plan_success) + + assert plan.plan_success.tolist() == [True, True] + # Both arms screen approach/grasp/lift plus the configured object-motion + # continuation, rather than jumping directly from lift to the final pose. + assert generator.robot.compute_batch_ik.call_count == 10 + left_held = projected.get_held_object("left_arm") + right_held = projected.get_held_object("right_arm") + assert left_held is not None and right_held is not None + torch.testing.assert_close( + left_held.grasp_xpos, + reachable.expand(NUM_ENVS, -1, -1), + ) + torch.testing.assert_close( + right_held.grasp_xpos, + reachable.expand(NUM_ENVS, -1, -1), + ) + + +def test_coordinated_pick_canonicalizes_parallel_jaw_half_turn() -> None: + """A sampled wrist-roll equivalent must not make a top grasp unreachable.""" + generator = _dual_motion_generator() + action = _bind_action( + generator, + CoordinatedPickment( + default_options=CoordinatedPickmentOptions( + hand_interp_steps=4, + hold_steps=2, + object_motion_keyframes=3, + ), + ), + ) + half_turn = torch.eye(4, dtype=torch.float32) + half_turn[0, 0] = -1.0 + half_turn[1, 1] = -1.0 + arm_result = { + "is_success": True, + "grasp_poses": half_turn.unsqueeze(0), + "open_lengths": torch.zeros(1), + "total_cost": torch.zeros(1), + } + _GRASP_GENERATORS[id(action)].get_dual_arm_valid_grasp_poses = Mock( + return_value=[ + {"left": arm_result, "right": arm_result} for _ in range(NUM_ENVS) + ] + ) + invocation = ActionInvocation( + skill_id="coordinated_pickment", + goal=CoordinatedPickGoal( + semantics=ObjectSemantics( + affordance=AntipodalAffordance(), + geometry={}, + label="tray", + entity_id="tray", + ), + object_target_pose=torch.eye(4), + object_initial_pose=torch.eye(4), + ), + binding=_dual_binding(action, "left", "right"), + motion_policy=MotionPolicy(sample_count=30), + ) + + plan = _plan_action(action, invocation, _dual_context()) + projected = plan.expected_effects.apply( + TaskState.empty(NUM_ENVS, "cpu"), + plan.plan_success, + ) + + assert plan.success_all + left_held = projected.get_held_object("left_arm") + right_held = projected.get_held_object("right_arm") + assert left_held is not None and right_held is not None + identity = torch.eye(4).expand(NUM_ENVS, -1, -1) + torch.testing.assert_close(left_held.grasp_xpos, identity) + torch.testing.assert_close(right_held.grasp_xpos, identity) + + +def test_coordinated_pick_searches_geometry_adaptive_partitions() -> None: + """The Atomic Action, not its caller, recovers a blocked tray partition.""" + generator = _dual_motion_generator() + action = _bind_action(generator, CoordinatedPickment()) + vertices = torch.tensor( + [ + [-0.2, -0.1, -0.02], + [-0.2, -0.1, 0.02], + [-0.2, 0.1, -0.02], + [-0.2, 0.1, 0.02], + [0.2, -0.1, -0.02], + [0.2, -0.1, 0.02], + [0.2, 0.1, -0.02], + [0.2, 0.1, 0.02], + ], + dtype=torch.float32, + ) + triangles = torch.tensor( + [[0, 1, 2], [1, 2, 3], [4, 5, 6], [5, 6, 7]], + dtype=torch.long, + ) + sampled_ratios: list[float] = [] + + def sample_with_blocked_preferred_partition( + *, + obj_poses: torch.Tensor, + middle_empty_ratio: float, + **_kwargs: object, + ) -> list[dict[str, dict[str, object]] | None]: + sampled_ratios.append(middle_empty_ratio) + if len(sampled_ratios) == 1: + return [None for _ in range(obj_poses.shape[0])] + arm = { + "is_success": True, + "grasp_poses": torch.eye(4, dtype=torch.float32).unsqueeze(0), + "open_lengths": torch.tensor([0.0], dtype=torch.float32), + "total_cost": torch.tensor([0.0], dtype=torch.float32), + } + return [{"left": arm, "right": arm} for _ in range(obj_poses.shape[0])] + + _GRASP_GENERATORS[id(action)].get_dual_arm_valid_grasp_poses = Mock( + side_effect=sample_with_blocked_preferred_partition + ) + invocation = ActionInvocation( + skill_id="coordinated_pickment", + goal=CoordinatedPickGoal( + semantics=ObjectSemantics( + affordance=AntipodalAffordance( + mesh_vertices=vertices, + mesh_triangles=triangles, + ), + geometry={}, + label="tray", + entity_id="tray", + ), + object_target_pose=torch.eye(4), + object_initial_pose=torch.eye(4), + ), + binding=_dual_binding(action, "left", "right"), + motion_policy=MotionPolicy(sample_count=30), + ) + + plan = _plan_action(action, invocation, _dual_context()) + + assert plan.plan_success.tolist() == [True, True] + assert len(sampled_ratios) == 2 + assert sampled_ratios[0] != pytest.approx(0.4) + assert sampled_ratios[1] == pytest.approx(0.4) + + +def test_coordinated_pick_keeps_axis_aligned_partition_for_rotated_object() -> None: + """A rotated tray retains a geometry-only end-grasp partition fallback.""" + vertices = torch.tensor( + [ + [-0.2, -0.1, -0.02], + [-0.2, -0.1, 0.02], + [-0.2, 0.1, -0.02], + [-0.2, 0.1, 0.02], + [0.2, -0.1, -0.02], + [0.2, -0.1, 0.02], + [0.2, 0.1, -0.02], + [0.2, 0.1, 0.02], + ], + dtype=torch.float32, + ) + angle = torch.tensor(torch.pi / 6.0) + pose = torch.eye(4, dtype=torch.float32).unsqueeze(0) + pose[0, 0, 0] = torch.cos(angle) + pose[0, 0, 1] = -torch.sin(angle) + pose[0, 1, 0] = torch.sin(angle) + pose[0, 1, 1] = torch.cos(angle) + + ratios = CoordinatedPickment._candidate_middle_empty_ratios( + AntipodalAffordance( + mesh_vertices=vertices, + mesh_triangles=torch.tensor( + [[0, 1, 2], [1, 2, 3], [4, 5, 6], [5, 6, 7]], + dtype=torch.long, + ), + ), + pose, + torch.tensor([1.0, 0.0, 0.0]), + base_ratio=0.4, + ) + + assert len(ratios) == 5 + assert ratios[0] < ratios[1] + assert ratios[1] == pytest.approx(0.6) + assert ratios[2] == pytest.approx(0.4) + + +def test_coordinated_pick_can_release_both_hands_and_retreat() -> None: + generator = _dual_motion_generator() + action = _bind_action( + generator, + CoordinatedPickment( + default_options=CoordinatedPickmentOptions( + hand_interp_steps=4, + hold_steps=2, + release=True, + release_steps=4, + retreat_steps=5, + object_motion_keyframes=3, + ), + ), + ) + _stub_dual_arm_grasp_poses(action) + semantics = ObjectSemantics( + affordance=AntipodalAffordance(), + geometry={}, + label="shared-tray", + entity_id="tray", + ) + invocation = ActionInvocation( + skill_id="coordinated_pickment", + goal=CoordinatedPickGoal( + semantics=semantics, + object_target_pose=torch.eye(4), + object_initial_pose=torch.eye(4), + ), + binding=_dual_binding(action, "left", "right"), + motion_policy=MotionPolicy(sample_count=50), + ) + context = _dual_context() + + plan = _plan_action(action, invocation, context) + projected = plan.expected_effects.apply(context.task, plan.plan_success) + + assert plan.plan_success.tolist() == [True, True] + assert plan.commands.frame_count == 50 + assert tuple(segment.name for segment in plan.segments) == ( + "approach", + "close", + "lift", + "move", + "hold", + "release", + "retreat", + ) + assert torch.all(_joint_trajectory(plan).positions[:, -1, DUAL_ARM_DOF:] == 0.0) + assert projected.get_held_object("left_arm") is None + assert projected.get_held_object("right_arm") is None + + def test_assemble_place_uses_explicit_base_snapshot() -> None: generator = _motion_generator() action = _bind_action(generator, Place()) diff --git a/tests/sim/atomic_actions/test_core.py b/tests/sim/atomic_actions/test_core.py index 5811fa07d..48662b737 100644 --- a/tests/sim/atomic_actions/test_core.py +++ b/tests/sim/atomic_actions/test_core.py @@ -729,6 +729,87 @@ def test_scene_entity_pose_is_resolved_late_from_snapshot() -> None: assert collect_scene_dependencies(EndEffectorPoseGoal(reference)) == ("cup",) +def test_scene_entity_pose_applies_world_displacement_after_local_transform() -> None: + entity_pose = torch.eye(4).repeat(2, 1, 1) + entity_pose[:, :3, :3] = torch.tensor( + [ + [0.0, -1.0, 0.0], + [1.0, 0.0, 0.0], + [0.0, 0.0, 1.0], + ] + ) + local_offset = torch.eye(4) + local_offset[0, 3] = 0.2 + world_displacement = torch.tensor([-0.3, 0.0, 0.1]) + reference = SceneEntityPose( + "cup", + relative_pose=local_offset, + world_displacement=world_displacement, + ) + world_displacement.fill_(9.0) + context = _context( + SceneSnapshot( + timestamp=1.0, + version=3, + entities={"cup": EntityState(entity_pose, confidence=0.9)}, + ) + ) + + resolved = resolve_pose_goal(reference, context, name="xpos") + + torch.testing.assert_close( + resolved[:, :3, 3], + torch.tensor([[-0.3, 0.2, 0.1], [-0.3, 0.2, 0.1]]), + ) + snapshot = reference.snapshot() + assert snapshot.world_displacement is not reference.world_displacement + torch.testing.assert_close( + snapshot.world_displacement, + torch.tensor([-0.3, 0.0, 0.1]), + ) + + +def test_scene_entity_pose_applies_world_orientation_before_local_transform() -> None: + entity_pose = torch.eye(4).repeat(2, 1, 1) + entity_pose[:, :3, :3] = torch.tensor( + [ + [0.0, -1.0, 0.0], + [1.0, 0.0, 0.0], + [0.0, 0.0, 1.0], + ] + ) + entity_pose[:, :3, 3] = torch.tensor([[0.5, 0.2, 0.0], [0.7, 0.4, 0.0]]) + object_to_eef = torch.eye(4) + object_to_eef[2, 3] = 0.2 + world_orientation = torch.eye(3) + reference = SceneEntityPose( + "tray", + relative_pose=object_to_eef, + world_orientation=world_orientation, + world_displacement=torch.tensor([0.1, 0.0, 0.0]), + ) + world_orientation.fill_(9.0) + context = _context( + SceneSnapshot( + timestamp=1.0, + version=3, + entities={"tray": EntityState(entity_pose, confidence=0.9)}, + ) + ) + + resolved = resolve_pose_goal(reference, context, name="xpos") + + torch.testing.assert_close(resolved[:, :3, :3], torch.eye(3).repeat(2, 1, 1)) + torch.testing.assert_close( + resolved[:, :3, 3], + torch.tensor([[0.6, 0.2, 0.2], [0.8, 0.4, 0.2]]), + ) + assert collect_scene_dependencies(EndEffectorPoseGoal(reference)) == ("tray",) + snapshot = reference.snapshot() + assert snapshot.world_orientation is not reference.world_orientation + torch.testing.assert_close(snapshot.world_orientation, torch.eye(3)) + + def test_scene_entity_pose_enforces_confidence() -> None: context = _context( SceneSnapshot( diff --git a/tests/sim/atomic_actions/test_engine_per_env.py b/tests/sim/atomic_actions/test_engine_per_env.py index 0853631e5..b2543bc65 100644 --- a/tests/sim/atomic_actions/test_engine_per_env.py +++ b/tests/sim/atomic_actions/test_engine_per_env.py @@ -186,6 +186,24 @@ def _plan( ) +class FailedPhaseGateAction(PhaseGateAction): + """Fully failed planning attempt that never reaches a gated segment.""" + + skill_id: ClassVar[str] = "failed_phase_gate" + binding_contract: ClassVar[SkillBindingContract] = DynamicAction.binding_contract + + def _plan( + self, + request: ResolvedActionRequest[EndEffectorPoseGoal, ActionOptions], + context: PlanningContext, + ) -> ActionPlan: + return self.failed_plan( + request, + context, + message="No feasible grasp was found.", + ) + + class EffectAction(DynamicAction): """Dynamic test action that declares an attachment effect.""" @@ -846,13 +864,14 @@ def _destination_invocation( def _phase_gate_invocation( engine: AtomicActionEngine, *, + skill_id: str = PhaseGateAction.skill_id, segment_name: str = "commit", max_action_retries: int = 2, ) -> ActionInvocation[EndEffectorPoseGoal]: """Build a test invocation whose core owns one named segment gate.""" base = _invocation( engine, - skill_id=PhaseGateAction.skill_id, + skill_id=skill_id, max_action_retries=max_action_retries, ) return replace( @@ -950,6 +969,28 @@ def test_phase_effect_gate_requires_a_noninitial_named_segment( ) +def test_fully_failed_plan_preserves_diagnostics_before_phase_gate_validation() -> None: + engine, _ = _engine() + action = FailedPhaseGateAction() + engine.register(action) + + session = engine.start( + ( + _phase_gate_invocation( + engine, + skill_id=FailedPhaseGateAction.skill_id, + ), + ), + _context(0.0, 0.0, 0.2, 0), + ) + + assert not session.active_plan.plan_success.any() + assert session.active_plan.diagnostics.failure == PlanningFailure( + "planning_failed", + retryable=True, + ) + + def test_unresolved_phase_effect_gate_replays_preceding_command_for_full_cohort() -> ( None ): diff --git a/tests/toolkits/test_batch_convex_collision.py b/tests/toolkits/test_batch_convex_collision.py index 5e6255f4a..3a70b6778 100644 --- a/tests/toolkits/test_batch_convex_collision.py +++ b/tests/toolkits/test_batch_convex_collision.py @@ -30,6 +30,8 @@ pytestmark = pytest.mark.gpu +_EXPECTED_VHACD_MAX_SURFACE_DISTANCE = 0.5945 + def batch_convex_collision_query(device=torch.device("cuda")): mug_path = get_data_path("ScannedBottle/moliwulong_processed.ply") @@ -73,7 +75,10 @@ def batch_convex_collision_query(device=torch.device("cuda")): is_pose_collide = is_point_collide.any(dim=1) pose_surface_distance = point_surface_distance.min(dim=1).values assert is_pose_collide.sum().item() == 1 - assert abs(pose_surface_distance.max().item() - 0.8492) < 1e-2 + assert ( + abs(pose_surface_distance.max().item() - _EXPECTED_VHACD_MAX_SURFACE_DISTANCE) + < 1e-2 + ) def test_batch_convex_collision_cpu(): diff --git a/tests/toolkits/test_convex_collision_checker.py b/tests/toolkits/test_convex_collision_checker.py new file mode 100644 index 000000000..f0b0f3e3a --- /dev/null +++ b/tests/toolkits/test_convex_collision_checker.py @@ -0,0 +1,128 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +from __future__ import annotations + +import hashlib +from pathlib import Path + +import numpy as np +import pytest +import torch + +from embodichain.toolkits.graspkit.pg_grasp import collision_checker as module +from embodichain.toolkits.graspkit.pg_grasp.collision_checker import ( + ConvexCollisionChecker, +) + + +def _tetrahedron() -> tuple[np.ndarray, np.ndarray]: + vertices = np.array( + [ + [0.0, 0.0, 0.0], + [1.0, 0.0, 0.0], + [0.0, 1.0, 0.0], + [0.0, 0.0, 1.0], + ], + dtype=np.float32, + ) + faces = np.array( + [ + [0, 2, 1], + [0, 1, 3], + [0, 3, 2], + [1, 2, 3], + ], + dtype=np.int32, + ) + return vertices, faces + + +def test_plane_equations_use_vhacd(monkeypatch: pytest.MonkeyPatch) -> None: + vertices, faces = _tetrahedron() + calls: list[int] = [] + + def fake_vhacd(mesh, *, max_convex_hull_num: int): + calls.append(max_convex_hull_num) + return True, (mesh,) + + monkeypatch.setattr(module, "convex_decomposition_vhacd", fake_vhacd) + + plane_equations = ConvexCollisionChecker._compute_plane_equations( + vertices, + faces, + max_decomposition_hulls=16, + ) + + assert calls == [16] + assert len(plane_equations) == 1 + + +def test_vhacd_failure_is_reported(monkeypatch: pytest.MonkeyPatch) -> None: + vertices, faces = _tetrahedron() + + monkeypatch.setattr( + module, + "convex_decomposition_vhacd", + lambda *_args, **_kwargs: (False, ()), + ) + + with pytest.raises(RuntimeError, match="V-HACD convex decomposition failed"): + ConvexCollisionChecker._compute_plane_equations( + vertices, + faces, + max_decomposition_hulls=16, + ) + + +def test_vhacd_cache_does_not_reuse_legacy_backend( + monkeypatch: pytest.MonkeyPatch, + tmp_path: Path, +) -> None: + vertices, faces = _tetrahedron() + mesh_hash = hashlib.md5(vertices.tobytes() + faces.tobytes()).hexdigest() + legacy_path = tmp_path / f"{mesh_hash}_16.pkl" + legacy_path.write_bytes(b"legacy CoACD cache") + calls: list[int] = [] + + def fake_plane_equations( + _vertices: np.ndarray, + _faces: np.ndarray, + max_decomposition_hulls: int, + ) -> list[tuple[np.ndarray, np.ndarray]]: + calls.append(max_decomposition_hulls) + return [ + ( + np.array([[1.0, 0.0, 0.0]], dtype=np.float32), + np.array([0.0], dtype=np.float32), + ) + ] + + monkeypatch.setattr(module, "CONVEX_DECOMPOSITION_CACHE_DIR", tmp_path) + monkeypatch.setattr( + ConvexCollisionChecker, + "_compute_plane_equations", + staticmethod(fake_plane_equations), + ) + + checker = ConvexCollisionChecker( + torch.from_numpy(vertices), + torch.from_numpy(faces), + max_decomposition_hulls=16, + ) + + assert calls == [16] + assert checker.cache_path == str(tmp_path / f"{mesh_hash}_16_vhacd_v1.pkl") diff --git a/tests/toolkits/test_parallel_jaw_grasp_pose_generator.py b/tests/toolkits/test_parallel_jaw_grasp_pose_generator.py index b4ed9ffcb..bf56e3f91 100644 --- a/tests/toolkits/test_parallel_jaw_grasp_pose_generator.py +++ b/tests/toolkits/test_parallel_jaw_grasp_pose_generator.py @@ -223,6 +223,50 @@ def test_prepare_mesh_reuses_backend_and_returns_owned_pairs( assert torch.equal(second, torch.ones(1, 2, 3)) +def test_candidate_metadata_keeps_pose_width_and_cost_aligned( + backend: type[_Backend], monkeypatch: pytest.MonkeyPatch +) -> None: + def candidates(self, **kwargs): + poses = kwargs["object_pose"].repeat(2, 1, 1) + poses[:, 0, 3] += torch.tensor([0.1, 0.2]) + return True, poses, torch.tensor([0.02, 0.06]), torch.tensor([4.0, 1.0]) + + monkeypatch.setattr(backend, "get_valid_grasp_poses", candidates) + vertices, triangles = _geometry() + generator = _generator() + kwargs = dict( + mesh_vertices=vertices, + mesh_triangles=triangles, + obj_poses=torch.eye(4).repeat(2, 1, 1), + approach_direction=torch.tensor([0.0, 0.0, -1.0]), + ) + rows = generator.get_grasp_candidates(**kwargs) + legacy_rows = generator.get_valid_grasp_poses(**kwargs) + assert len(rows) == 2 + for (poses, widths, costs), (legacy_poses, legacy_costs) in zip(rows, legacy_rows): + assert torch.allclose(poses[:, 0, 3], torch.tensor([0.1, 0.2])) + assert torch.equal(widths, torch.tensor([0.02, 0.06])) + assert torch.equal(costs, torch.tensor([4.0, 1.0])) + assert torch.equal(poses, legacy_poses) + assert torch.equal(costs, legacy_costs) + + +def test_candidate_metadata_preserves_failed_row_sentinel( + backend: type[_Backend], +) -> None: + vertices, triangles = _geometry() + rows = _generator().get_grasp_candidates( + mesh_vertices=vertices, + mesh_triangles=triangles, + obj_poses=torch.eye(4)[None], + approach_direction=torch.tensor([0.0, 0.0, -1.0]), + ) + poses, widths, costs = rows[0] + assert poses.shape == (1, 4, 4) + assert widths.shape == costs.shape == (1,) + assert torch.isinf(costs).all() + + def test_direct_best_grasp_configures_and_reuses_private_mesh_backend( backend: type[_Backend], ) -> None: @@ -253,6 +297,7 @@ def test_direct_best_grasp_configures_and_reuses_private_mesh_backend( assert sampler_cfg.n_sample == 321 assert sampler_cfg.min_length == 0.004 assert sampler_cfg.max_length == 0.12 + assert sampler_cfg.center_mode == "bounds" assert configured.options["max_candidates"] == 7 assert configured.options["filter_ground_collision"] is False assert collision_cfg.finger_length == 0.09 @@ -295,6 +340,7 @@ def test_backend_cache_key_includes_sampling_and_annotation_policy() -> None: vertices, triangles = _geometry() generator = object.__new__(_AntipodalMeshBackend) generator._sampler_cfg = AntipodalSamplerCfg() + generator._center_mode = generator._sampler_cfg.center_mode generator._interactive_annotation = False generator._use_largest_connected_component = False baseline = generator._get_cache_dir(vertices, triangles) diff --git a/tests/toolkits/test_pg_grasp.py b/tests/toolkits/test_pg_grasp.py index dc396bebf..6a84fbdef 100644 --- a/tests/toolkits/test_pg_grasp.py +++ b/tests/toolkits/test_pg_grasp.py @@ -60,13 +60,14 @@ def _box_geometry(*, subdivide_end: bool = False) -> tuple[torch.Tensor, torch.T def _prepared_backend( - vertices: torch.Tensor, pairs: torch.Tensor + vertices: torch.Tensor, pairs: torch.Tensor, *, center_mode: str = "bounds" ) -> _AntipodalMeshBackend: """Bypass sampling and collision construction to test prepared-pair logic.""" backend = _AntipodalMeshBackend.__new__(_AntipodalMeshBackend) backend.device = vertices.device backend.vertices = vertices backend._hit_point_pairs = pairs + backend._center_mode = center_mode return backend @@ -112,6 +113,33 @@ def test_default_grasp_mode_ranks_by_transformed_bounding_box_center() -> None: assert costs.min() == pytest.approx(0.0, abs=1.0e-6) +def test_centroid_mode_preserves_vertex_weighted_ranking() -> None: + vertices, _ = _box_geometry(subdivide_end=True) + centers = torch.tensor([[0.0, 0.0, 0.0], [0.08, 0.0, 0.0]]) + contact_offset = torch.tensor([0.0, 0.02, 0.0]) + pairs = torch.stack([centers - contact_offset, centers + contact_offset], dim=1) + backend = _prepared_backend(vertices, pairs, center_mode="centroid") + backend._max_deviation_angle = 0.1 + backend._approach_direction_samples = 1 + backend._max_candidates = 10 + backend._filter_ground_collision = False + backend._collision_checker = Mock() + backend._collision_checker.query.return_value = ( + torch.zeros(len(pairs), dtype=torch.bool), + torch.zeros(len(pairs)), + ) + object_pose = _object_pose() + + success, poses, _, costs = backend.get_valid_grasp_poses( + object_pose=object_pose, + approach_direction=torch.tensor([0.0, 0.0, -1.0]), + ) + + assert success + expected = centers[1] @ object_pose[:3, :3].T + object_pose[:3, 3] + torch.testing.assert_close(poses[costs.argmin(), :3, 3], expected) + + @pytest.mark.parametrize( "middle_empty_ratio, side_count", [(0.0, 7), (0.4, 4), (0.8, 1)] ) @@ -173,6 +201,35 @@ def test_dual_arm_gap_excludes_middle_and_boundaries_by_pair_center( torch.testing.assert_close(call.kwargs["mesh_center"], expected_center) +def test_centroid_dual_arm_partition_does_not_duplicate_crossing_pairs( + monkeypatch: pytest.MonkeyPatch, +) -> None: + vertices, _ = _box_geometry() + vertices[:, 0] *= 10.0 + backend = _prepared_backend( + vertices, + # Both contacts lie beyond the gap, but their center lies inside it. + torch.tensor([[[-0.9, 0.0, 0.0], [0.9, 0.0, 0.0]]]), + center_mode="centroid", + ) + filter_poses = Mock( + return_value=(True, torch.eye(4)[None], torch.ones(1), torch.zeros(1)) + ) + monkeypatch.setattr(backend, "_filter_valid_grasp_poses", filter_poses) + + result = backend.get_dual_arm_valid_grasp_poses( + object_pose=torch.eye(4), + approach_direction=torch.tensor([0.0, 0.0, -1.0]), + left_to_right_arm_direction=torch.tensor([1.0, 0.0, 0.0]), + middle_empty_ratio=0.8, + ) + + assert result is not None + assert filter_poses.call_count == 2 + assert filter_poses.call_args_list[0].kwargs["origin_points_"].numel() == 0 + assert filter_poses.call_args_list[1].kwargs["origin_points_"].numel() == 0 + + def _raycast_box(*, subdivide_end: bool) -> tuple[torch.Tensor, torch.Tensor]: vertices, triangles = _box_geometry(subdivide_end=subdivide_end) sampler = AntipodalSampler()