diff --git a/agent_context/topics/atomic-actions/atomic-actions.md b/agent_context/topics/atomic-actions/atomic-actions.md index ec1489b17..70d1fb642 100644 --- a/agent_context/topics/atomic-actions/atomic-actions.md +++ b/agent_context/topics/atomic-actions/atomic-actions.md @@ -472,6 +472,13 @@ command-only `RuntimeEndpointTarget`. This keeps motion, mobile, whole-body, articulation, and custom controller transports extensible without treating a control part as symbolic state identity. +`HeldObjectState` is verified symbolic knowledge only. Neither it nor the +standard effect runtime creates simulator joints, managed attachments, +kinematic parents, frozen bodies, or pose overrides. Physical grasp retention +therefore depends on the configured controller, collision geometry, materials, +contact solver, and rigid-body parameters. A command-state evidence value is +only accepted controller intent and never physical contact proof by itself. + Providers emit raw `PoseRelationEvidenceBatch`, `BinaryEffectEvidenceBatch`, `ScalarEffectEvidenceBatch`, or `JointStateEvidenceBatch` values with stable environment IDs, per-row validity/acquisition diagnostics, timestamps, and @@ -491,6 +498,17 @@ Cause events (`ACTION_PLANNING_FAILED`, `EFFECT_VERIFICATION_FAILED`, and `EFFECT_VERIFICATION_TIMEOUT`) are distinct from the `ACTION_RETRY` recovery event. `SESSION_COMPLETED` and `SESSION_FAILED` are distinct terminal events. +Effect verification is currently a terminal action boundary. There is no +in-flight physical-invariant monitor for the held-object relation during Pick +lift or HandOver transfer/release/delivery. A slip can therefore be detected at +the terminal monitor but cannot interrupt the trajectory at the frame where it +occurs. On a failed HandOver, the success-only `StateDelta` is not committed, +but an already verified source-held relation also is not reconciled from +failure evidence; blindly retrying after both grippers lost the object can use +stale symbolic state. Pure-dynamics recovery needs a typed, phase-aware +in-flight guard plus failure-outcome reconciliation rather than a simulator-side +attachment. + Recovery replans reuse the current immutable `ResolvedActionRequest`, including its owned goal snapshot. Mutable goal values are copied, while simulator-backed `BatchEntity` handles retain their runtime identity. To change a goal, option, @@ -693,7 +711,13 @@ profile catalog rather than duplicate robot data across tasks. The Open Drawer vertical slice has completed its supported-simulation physical run and reached the configured drawer joint target. Repeated cube pick/place has completed one physical Pick/Place/settle/validator cycle; the full three-cycle -run remains in threshold calibration. +run remains in threshold calibration. The dual-UR5/PGI HandOver slice has +completed three consecutive supported-simulation Pick/transfer/settle/validator +runs using contact dynamics only. Its calibrated profile drives only the PGI +master joints, keeps mimic-child drives disabled, uses a 0.011 close target with +stiffness 2000, damping 50, and maximum effort 140, models the can at 0.33 kg, +and uses 200 motion samples. The default 0.05-rad tracking gate and bounded +replanning remain active. When no explicit contact or constraint callback is installed, simulation grasp and release evidence combines the live object-to-endpoint pose relation with @@ -707,9 +731,28 @@ contact by itself. `DynamicSettleMonitor` is shared by reset events and the Expert Program `wait_stable` post-policy. It owns threshold, cadence, consecutive-check, -settled, and timeout state but never steps simulation. The demo policy yields -full-qpos holds through the normal environment step path. Segment validators -remain a separate dataset/task boundary. +settled, and timeout state but never steps simulation. Eligible rows reuse live +target qpos so a contact-blocked position gripper retains closure preload; +initially inactive rows use fresh measured-qpos holds. Early-settled eligible +rows keep their targets until the active cohort terminates. Every action still +passes through the normal environment-step path, and segment validators remain +a separate dataset/task boundary. + +The standard simulation factory lowers both `MotionPolicy.control_dt` and +`ExecutionRunnerCfg.minimum_cycle_time` to the authoritative Gym `step_dt`. +When `hold_during_effect_verification=False`, runner polling emits no +observed-position HOLD; the bridge advances physics by replaying the last +accepted environment action. HandOver also sets `hold_on_completion=False`, so +its subsequent `wait_stable` policy continues the existing targets rather than +neutralizing the gripper at its contact-displaced qpos. Cancellation and +failure still perform cancel followed by an observed-position safe hold. + +This staged B behavior solves the validated joint-position HandOver path but is +not a generic continuation contract for mobile-base or whole-body transports. +Those endpoints need a typed transport-owned continuation command rather than a +joint-qpos latch. `wait_stable` also runs only after terminal effect verification +and symbolic-state commit, so its timeout is a post-policy failure and does not +trigger atomic-action recovery. Runtime and demo results expose deterministic JSON-safe metadata. Call traces include invocation identity, masks, command counts, execution/recovery events, diff --git a/docs/design/declarative_expert_program_plan.md b/docs/design/declarative_expert_program_plan.md index e52240826..2cfb53d98 100644 --- a/docs/design/declarative_expert_program_plan.md +++ b/docs/design/declarative_expert_program_plan.md @@ -6,8 +6,10 @@ tests are explicitly enabled. Open Drawer has completed its supported-simulation physical run; repeated cube pick/place has completed one Pick/Place/settle/validator cycle, while the full three-cycle run remains in - threshold calibration. -- Baseline: `main@bcccb787e8f9165e9c8acf6f39f165ba6ac752a4` + threshold calibration. Dual-UR5/PGI HandOver has completed three consecutive + supported-simulation Pick/transfer/settle/validator runs using contact + dynamics only. +- Baseline: `main@bcccb787dcafdafd7b944ba210e5e85f9cd1d0cb` - Last updated: 2026-08-11 - Related issues: [#471](https://github.com/DexForce/EmbodiChain/issues/471), [#474](https://github.com/DexForce/EmbodiChain/issues/474) @@ -586,6 +588,58 @@ handover, and articulation-joint progress. Hardware can implement the same contract with perception, force, or controller feedback. Custom monitors stay an advanced extension point. +Grasp and handover must remain real dynamics outcomes. A simulation effect +monitor is observational: it must not create a fixed joint, managed attachment, +kinematic parent, frozen body, or pose override to make a held-object relation +persist. Grasp retention comes from embodiment-owned drive settings, collision +geometry, material/contact parameters, solver settings, and the commands sent +through the normal controller path. An accepted semantic ``grasp`` command is +controller intent, not physical proof. + +``HeldObjectState`` records a relation only after live physical evidence has +passed the selected monitor. The target contract must treat contradictory +evidence, including object-to-endpoint slip, as a real effect failure, invalidate +the affected row's assumed relation, and enter bounded recovery instead of +repairing the scene. The runtime now exposes the active named motion phase, +observes phase-scoped held-object invariants from fresh physical evidence, and +applies removal-only ``StateDelta`` reconciliation to failed rows before any +retry or recovery hand-off. ``Pick`` can use the existing bounded action retry; +``Place`` and ``HandOver`` currently emit a typed ``RECOVERY_REQUIRED`` boundary +because replaying the same invocation after its required relation was removed +would be invalid. A workflow-level re-acquisition policy, blocking acquisition +gates, per-expectation terminal failure reconciliation, and fail-closed +reconciliation for evidence that remains unresolved at the action deadline +remain explicit design decisions rather than implicit scene repair. For +handover, success transfers the verified relation from source to destination +while the destination remains physically closed. Releasing the destination is +a separate ``Place`` or ``Release`` semantic call. + +The first pure-dynamics rollout uses the staged **B** continuation policy. The +standard simulation factory lowers both trajectory ``control_dt`` and runner +``minimum_cycle_time`` to the authoritative Gym step. A persistent +joint-position task may disable observed-position holds during terminal effect +verification and on successful completion; bridge wait steps then replay the +last accepted environment action, and a following ``wait_stable`` policy keeps +eligible rows on their live drive targets. Cancellation and failure retain the +normal cancel-then-observed-position safe stop. This split preserves physical +gripper preload without converting a success continuation into a universal +safe-state policy. + +The validated dual-UR5/PGI slice drives only each PGI master joint (the mimic +child drive is disabled), uses a ``0.011`` close target with +``stiffness=2000``, ``damping=50``, and ``max_effort=140``, models the can at +``0.33 kg``, and executes a 200-sample motion policy. These values are task and +embodiment calibration, not effect-monitor success shortcuts: the normal +``0.05 rad`` tracking gate, bounded replanning, physical effect evidence, and +settling thresholds remain enabled. + +The B policy is the complete continuation scope of this refactor. A generic +mobile-base or whole-body continuation abstraction is deliberately excluded +from the implementation plan and acceptance checklist. If a later transport +requires persistence beyond its normal command contract, it should be proposed +and validated independently instead of becoming a blocker for the declarative +expert-program rollout. + ## 8. Expert Program configuration ### 8.1 Version 1 schema @@ -1165,10 +1219,16 @@ The backend-neutral typed state expectations, evidence addresses and sources, pose/binary/scalar/joint evidence clauses, versioned monitor registry, profile-owned monitor selection, grounded Pick/Place/HandOver/articulation effects, row-local composite hysteresis kernel, canonical `SkillRuntime`, and -production simulation evidence ports are wired end to end. Physical simulation -acceptance is partial: Open Drawer and one cube Pick/Place/settle/validator -cycle have completed, while the full repeated-cube run and embodiment-owned -HandOver pose integration remain validation work. +production simulation evidence ports are wired end to end. Phase-scoped +held-object guard requests, live evidence collection, row-local symbolic +invalidation, bounded Pick retry, and typed external-recovery hand-off are also +implemented. Physical simulation acceptance is partial: Open Drawer and one +cube Pick/Place/settle/validator cycle have completed. The embodiment-owned +dual-UR5/PGI HandOver slice now completes Pick, transfer, terminal +physical-effect verification, settling, and target validation through real +contact dynamics; blocking acquisition gates, workflow-level re-acquisition, +per-expectation terminal reconciliation, fault-injection coverage, and the full +repeated-cube run remain validation or design work. Deliverables: @@ -1416,7 +1476,19 @@ The design is complete when all of the following hold: goals without caller duplication. - [x] `Place` is object-centric and consumes verified held-object state. - [ ] Built-in grasp, release, handover, and supported articulation effect - monitors work in simulation. + monitors work in simulation. The dual-UR5/PGI HandOver vertical slice is + physically validated; remaining skill/embodiment coverage keeps this + aggregate item open. +- [ ] Grasp and handover simulation gates retain objects through configured + drive/contact dynamics only; no monitor or runtime path creates a + synthetic attachment, freezes the object, or overrides its pose. +- [ ] Physical held-object loss is observed as effect failure, invalidates the + affected symbolic relation, and exercises bounded recovery rather than + being hidden by a simulator-side attachment. The phase-aware observation, + row-local invalidation, bounded Pick retry, and typed recovery boundary + are implemented; blocking acquisition, per-expectation terminal + reconciliation, workflow-level re-acquisition, and real-simulation fault + injection remain open. - [x] Repeated sub-threshold motion eventually publishes the correct scene revision. - [x] Custom actions have a documented and tested intentional hard-break diff --git a/docs/source/overview/sim/atomic_actions/index.md b/docs/source/overview/sim/atomic_actions/index.md index 6ad3ae46c..d1e5fe56d 100644 --- a/docs/source/overview/sim/atomic_actions/index.md +++ b/docs/source/overview/sim/atomic_actions/index.md @@ -825,6 +825,23 @@ and resets the history. Evidence exactly at the deadline is valid, while a due observation after the deadline is handled by session timeout without invoking the verifier. +The curated semantic runtime also installs phase-scoped, negative +held-object guards for named trajectory segments. Before a due command is +dispatched, `ExecutionRunner` passes a fresh observation and the current +`HeldObjectGuardRequest` to its synchronous guard verifier. Each request has a +single-use verification ID, the active waypoint/segment identity, and the +action-owned symbolic key/object identities that may be invalidated. A +contradictory result must name that canonical object and carry a removal-only +`StateDelta`; `ExecutionSession` applies that delta to only the failed rows +before retrying or emitting `RECOVERY_REQUIRED`. +Unavailable or unresolved evidence does not count as a physical contradiction, +and the guard verifier is not invoked after the authoritative action deadline. + +The current guard is observational and negative; a blocking positive +acquisition gate, outcome-aware terminal reconciliation, and workflow-level +re-acquisition remain separate policies. Neither the monitor nor runtime +creates a simulator attachment, freezes an object, or overrides its pose. + ## Action Agent integration An MLLM should not construct `ActionInvocation` by copying arbitrary JSON into diff --git a/embodichain/lab/gym/envs/expert_program/__init__.py b/embodichain/lab/gym/envs/expert_program/__init__.py index a3cae5178..43eec1892 100644 --- a/embodichain/lab/gym/envs/expert_program/__init__.py +++ b/embodichain/lab/gym/envs/expert_program/__init__.py @@ -152,6 +152,7 @@ SimulationPlanningObservationProvider, create_simulation_expert_program_adapter, ) +from .simulation_handover import ConfiguredHandOverPoseProvider from .simulation_policies import ( SimulationSegmentPolicyPort, default_simulation_settle_presets, @@ -183,6 +184,7 @@ "ControlPartCommandPreset", "ControlPartEndpointBinding", "ControlPartResourceBinding", + "ConfiguredHandOverPoseProvider", "CyclicPoseTargetCfg", "DeclarativeCfgValue", "DemoBridgeError", diff --git a/embodichain/lab/gym/envs/expert_program/catalog.py b/embodichain/lab/gym/envs/expert_program/catalog.py index 8db7f334a..79ea658d9 100644 --- a/embodichain/lab/gym/envs/expert_program/catalog.py +++ b/embodichain/lab/gym/envs/expert_program/catalog.py @@ -1092,22 +1092,25 @@ def validate_bound_endpoint_extensions( ) -def _profile_with_control_dt( +def _profile_with_step_dt( profile: RobotSkillProfile, *, - control_dt: float, + step_dt: float, ) -> RobotSkillProfile: - """Return the registration profile aligned to one Gym control cadence.""" + """Return the registration profile aligned to one Gym runtime cadence.""" return replace( profile, presets={ preset_id: SkillPolicyPreset( preset_id=preset.preset_id, schema_version=preset.schema_version, - motion_policy=replace(preset.motion_policy, control_dt=control_dt), + motion_policy=replace(preset.motion_policy, control_dt=step_dt), tracking_policy=preset.tracking_policy, recovery_policy=preset.recovery_policy, - runner_cfg=preset.runner_cfg, + runner_cfg=replace( + preset.runner_cfg, + minimum_cycle_time=step_dt, + ), effect_monitors=preset.effect_monitors, action_option_templates=preset.action_option_templates, ) @@ -1476,9 +1479,9 @@ def validate_robot_profile( self.assert_unchanged() if type(profile) is not RobotSkillProfile: raise TypeError("profile must be exactly RobotSkillProfile.") - expected = _profile_with_control_dt( + expected = _profile_with_step_dt( self.catalog.robot_profile, - control_dt=step_dt, + step_dt=step_dt, ) if _canonical_json(profile) != _canonical_json(expected): raise IntegrationFingerprintMismatch( diff --git a/embodichain/lab/gym/envs/expert_program/simulation_environment.py b/embodichain/lab/gym/envs/expert_program/simulation_environment.py index abcacd901..0a073c9b2 100644 --- a/embodichain/lab/gym/envs/expert_program/simulation_environment.py +++ b/embodichain/lab/gym/envs/expert_program/simulation_environment.py @@ -718,10 +718,11 @@ class SimulationExpertProgramFactory(ExpertProgramEnvironmentFactory): translation_threshold: Material scene translation threshold. rotation_threshold: Material scene rotation threshold. - Every profile policy is rebuilt with ``control_dt == step_dt``. The Gym - cadence is authoritative because commands cannot be emitted between - environment steps; silently retaining a preset's unrelated fallback - cadence would make trajectory timing unrepresentable at the bridge. + Every profile policy is rebuilt with ``control_dt == step_dt`` and + ``minimum_cycle_time == step_dt``. The Gym cadence is authoritative because + commands and fresh feedback cannot be produced between environment steps; + silently retaining a preset's unrelated fallback cadence would make runtime + timing unrepresentable at the bridge. """ def __init__( @@ -872,7 +873,7 @@ def create_scene_registry(self) -> SceneRegistry: return registry def create_robot_skill_profile(self) -> RobotSkillProfile: - """Build a profile whose every motion policy uses the Gym cadence.""" + """Build a profile whose motion and runner policies use Gym cadence.""" profile = self._robot_profile_binding.build(self._robot) aligned_presets = { preset_id: SkillPolicyPreset( @@ -884,7 +885,10 @@ def create_robot_skill_profile(self) -> RobotSkillProfile: ), tracking_policy=preset.tracking_policy, recovery_policy=preset.recovery_policy, - runner_cfg=preset.runner_cfg, + runner_cfg=replace( + preset.runner_cfg, + minimum_cycle_time=self._step_dt, + ), effect_monitors=preset.effect_monitors, action_option_templates=preset.action_option_templates, ) @@ -893,9 +897,10 @@ def create_robot_skill_profile(self) -> RobotSkillProfile: aligned = replace(profile, presets=aligned_presets) if any( preset.motion_policy.control_dt != self._step_dt + or preset.runner_cfg.minimum_cycle_time != self._step_dt for preset in aligned.presets.values() ): - raise AssertionError("Profile motion policies were not cadence-aligned.") + raise AssertionError("Profile runtime policies were not cadence-aligned.") self._registration.validate_robot_profile( aligned, step_dt=self._step_dt, diff --git a/embodichain/lab/gym/envs/expert_program/simulation_handover.py b/embodichain/lab/gym/envs/expert_program/simulation_handover.py new file mode 100644 index 000000000..9ac4d6f50 --- /dev/null +++ b/embodichain/lab/gym/envs/expert_program/simulation_handover.py @@ -0,0 +1,146 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Configured simulation integration for semantic hand-over poses.""" + +from __future__ import annotations + +from dataclasses import dataclass +from typing import TYPE_CHECKING, ClassVar + +from embodichain.lab.sim.skills import ( + HandOverPoseProvider, + HandOverPoseTargets, + SemanticObjectTarget, + SemanticPose, +) + +if TYPE_CHECKING: + from embodichain.lab.sim.atomic_actions import PlanningContext + from embodichain.lab.sim.skills import BoundSemanticCall, HandOver + + +def _validated_pose( + position: tuple[float, float, float], + quaternion_wxyz: tuple[float, float, float, float], + *, + field_name: str, +) -> SemanticPose: + """Build and validate one unbatched semantic pose declaration.""" + if type(position) is not tuple or len(position) != 3: + raise TypeError(f"{field_name}_position must be an exact 3-tuple.") + if type(quaternion_wxyz) is not tuple or len(quaternion_wxyz) != 4: + raise TypeError(f"{field_name}_quaternion_wxyz must be an exact 4-tuple.") + try: + return SemanticPose( + position=position, + quaternion_wxyz=quaternion_wxyz, + ) + except (TypeError, ValueError) as exc: + raise type(exc)(f"Invalid {field_name} hand-over pose: {exc}") from exc + + +@dataclass(frozen=True, slots=True) +class ConfiguredHandOverPoseProvider(HandOverPoseProvider): + """Resolve hand-over targets from immutable embodiment configuration. + + The provider carries object-space poses rather than arm trajectories. The + shared semantic compiler and atomic ``HandOver`` implementation remain + responsible for grasp selection, IK, motion generation, transfer, release, + and delivery. Keeping the numeric declaration as tuple fields also makes + the provider suitable for task-registration catalog fingerprinting. + + Args: + middle_position: World-frame object position at transfer time. + middle_quaternion_wxyz: World-frame object orientation at transfer time. + final_position: World-frame object delivery position. + final_quaternion_wxyz: World-frame object delivery orientation. + """ + + provider_id: ClassVar[str] = "simulation.configured_handover_pose" + + middle_position: tuple[float, float, float] + middle_quaternion_wxyz: tuple[float, float, float, float] + final_position: tuple[float, float, float] + final_quaternion_wxyz: tuple[float, float, float, float] + + def __post_init__(self) -> None: + middle = _validated_pose( + self.middle_position, + self.middle_quaternion_wxyz, + field_name="middle", + ) + final = _validated_pose( + self.final_position, + self.final_quaternion_wxyz, + field_name="final", + ) + object.__setattr__( + self, + "middle_position", + tuple(float(value) for value in middle.position.tolist()), + ) + object.__setattr__( + self, + "middle_quaternion_wxyz", + tuple(float(value) for value in middle.quaternion_wxyz.tolist()), + ) + object.__setattr__( + self, + "final_position", + tuple(float(value) for value in final.position.tolist()), + ) + object.__setattr__( + self, + "final_quaternion_wxyz", + tuple(float(value) for value in final.quaternion_wxyz.tolist()), + ) + + def resolve( + self, + call: HandOver, + *, + context: PlanningContext, + bound: BoundSemanticCall, + ) -> HandOverPoseTargets: + """Return independently owned object-space transfer targets. + + Args: + call: Canonical hand-over semantic call. + context: Latest immutable planning observation. + bound: Engine/profile-bound hand-over call. + + Returns: + Configured middle and final object-space targets. + """ + del call, context, bound + return HandOverPoseTargets( + middle=SemanticObjectTarget( + pose=SemanticPose( + position=self.middle_position, + quaternion_wxyz=self.middle_quaternion_wxyz, + ) + ), + final=SemanticObjectTarget( + pose=SemanticPose( + position=self.final_position, + quaternion_wxyz=self.final_quaternion_wxyz, + ) + ), + ) + + +__all__ = ["ConfiguredHandOverPoseProvider"] diff --git a/embodichain/lab/gym/envs/expert_program/simulation_policies.py b/embodichain/lab/gym/envs/expert_program/simulation_policies.py index 408e7cac0..8795ce03a 100644 --- a/embodichain/lab/gym/envs/expert_program/simulation_policies.py +++ b/embodichain/lab/gym/envs/expert_program/simulation_policies.py @@ -19,8 +19,10 @@ The port in this module deliberately consumes the same explicit :class:`SimulationSceneBinding` used to construct the semantic scene registry. It never scans a simulation or guesses a native entity from a canonical name. -Post-policy actions are full-qpos holds and therefore remain inside the normal -Gym ``env.step()`` path owned by :class:`AtomicDemoBridge`. +Post-policy actions remain inside the normal Gym ``env.step()`` path owned by +:class:`AtomicDemoBridge`. Rows eligible for the policy reuse full drive targets +so physical contact does not erase position-control preload; rows already +inactive use fresh measured-position holds. """ from __future__ import annotations @@ -92,7 +94,8 @@ class SimulationSegmentPolicyPort: Args: simulation: Live simulation used only for UIDs declared in ``scene_binding``. - robot: Live robot used to produce controller-safe full-qpos holds. + robot: Live robot used to produce full target-qpos holds while the + post-policy observes settling. scene_binding: Exact canonical-to-native scene declaration. settle_presets: Named settling policies. ``None`` installs the shared ``rigid_object`` preset. @@ -116,7 +119,9 @@ def __init__( ) -> None: if type(scene_binding) is not SimulationSceneBinding: raise TypeError("scene_binding must be exactly SimulationSceneBinding.") - qpos = self._read_robot_qpos(robot) + qpos = self._read_robot_qpos(robot, target=False) + self._robot_qpos_shape = qpos.shape + self._robot_qpos_device = qpos.device if env_ids is None: env_ids = torch.arange( qpos.shape[0], @@ -221,7 +226,7 @@ def actions( segment: Any, active_mask: torch.Tensor, ) -> Iterator[torch.Tensor]: - """Yield full-qpos hold actions until active rows settle or time out. + """Yield full target-qpos hold actions until rows settle or time out. Args: policy: Exact compiled ``wait_stable`` policy. @@ -231,7 +236,10 @@ def actions( not participate in settling, timeout, or success results. Yields: - Fresh full-qpos hold commands consumed by ordinary ``env.step()``. + Fresh full target-qpos hold commands consumed by ordinary + ``env.step()``. Reading drive targets instead of measured joint + positions preserves contact preload in position-controlled tools. + Rows inactive when the policy starts use fresh measured qpos holds. Timeout is a normal row-local result boundary. Timed-out rows are exposed through :meth:`post_policy_result` and @@ -293,7 +301,7 @@ def actions( return if bool(state.timeout_mask.any().item()): return - yield self._read_robot_qpos(self._robot) + yield self._hold_robot_qpos(active_mask) elapsed_steps += 1 def post_policy_result( @@ -413,12 +421,16 @@ def validator_metadata( return deepcopy(metadata) @staticmethod - def _read_robot_qpos(robot: Robot) -> torch.Tensor: - """Capture one finite full-robot position batch.""" + def _read_robot_qpos(robot: Robot, *, target: bool) -> torch.Tensor: + """Capture one finite current- or target-qpos full-robot batch.""" + if type(target) is not bool: + raise TypeError("target must be a bool.") + mode = "target" if target else "current" + call = f"robot.get_qpos(target={target})" get_qpos = getattr(robot, "get_qpos", None) if not callable(get_qpos): - raise TypeError("robot must provide get_qpos().") - qpos = get_qpos() + raise TypeError(f"robot must provide {call}.") + qpos = get_qpos(target=target) if ( not isinstance(qpos, torch.Tensor) or not qpos.is_floating_point() @@ -426,11 +438,40 @@ def _read_robot_qpos(robot: Robot) -> torch.Tensor: or qpos.shape[0] == 0 or qpos.shape[1] == 0 ): - raise ValueError("robot.get_qpos() must return floating shape (B, J).") + raise ValueError( + f"{call} must return {mode} floating full-qpos shape (B, J)." + ) if not bool(torch.isfinite(qpos).all().item()): - raise ValueError("robot.get_qpos() must contain finite values.") + raise ValueError(f"{call} must return finite {mode} qpos values.") return qpos.clone() + def _hold_robot_qpos(self, active_mask: torch.Tensor) -> torch.Tensor: + """Keep initial active rows on targets and inactive rows on current qpos.""" + target_qpos = self._read_robot_qpos(self._robot, target=True) + if ( + target_qpos.shape != self._robot_qpos_shape + or target_qpos.device != self._robot_qpos_device + ): + raise ValueError( + "robot.get_qpos(target=True) target full qpos must match the " + "construction-time current full qpos shape and device." + ) + if bool(active_mask.all().item()): + return target_qpos + + current_qpos = self._read_robot_qpos(self._robot, target=False) + if ( + current_qpos.shape != self._robot_qpos_shape + or current_qpos.device != self._robot_qpos_device + ): + raise ValueError( + "robot.get_qpos(target=False) current full qpos must match the " + "construction-time current full qpos shape and device." + ) + hold_qpos = current_qpos.clone() + hold_qpos[active_mask] = target_qpos[active_mask] + return hold_qpos + def _validate_active_mask(self, active_mask: torch.Tensor) -> torch.Tensor: """Return one owned row mask aligned with the simulator batch.""" if not isinstance(active_mask, torch.Tensor): diff --git a/embodichain/lab/sim/atomic_actions/__init__.py b/embodichain/lab/sim/atomic_actions/__init__.py index 8611766fb..681092706 100644 --- a/embodichain/lab/sim/atomic_actions/__init__.py +++ b/embodichain/lab/sim/atomic_actions/__init__.py @@ -67,6 +67,8 @@ ExecutionSession, ExecutionStatus, ExecutionTick, + HeldObjectGuardRequest, + HeldObjectGuardResult, ) from .goals import ( ActionGoal, @@ -190,6 +192,7 @@ ExecutionClock, ExecutionRunner, ExecutionRunnerCfg, + HeldObjectGuardVerifier, MonotonicExecutionClock, ObservationProvider, RunnerStatus, @@ -272,6 +275,9 @@ "ExecutionSession", "ExecutionStatus", "ExecutionTick", + "HeldObjectGuardRequest", + "HeldObjectGuardResult", + "HeldObjectGuardVerifier", "EndpointTrackingChannelBinding", "EndpointTrackingFeedbackAddress", "FeedbackTerminalAcceptance", diff --git a/embodichain/lab/sim/atomic_actions/execution.py b/embodichain/lab/sim/atomic_actions/execution.py index 4876a0c25..1c251ef54 100644 --- a/embodichain/lab/sim/atomic_actions/execution.py +++ b/embodichain/lab/sim/atomic_actions/execution.py @@ -74,8 +74,10 @@ class ExecutionEventKind(str, Enum): EFFECT_VERIFICATION_REQUIRED = "effect_verification_required" EFFECT_VERIFICATION_FAILED = "effect_verification_failed" EFFECT_VERIFICATION_TIMEOUT = "effect_verification_timeout" + HELD_OBJECT_LOST = "held_object_lost" ACTION_RETRY = "action_retry" ACTION_COMPLETED = "action_completed" + RECOVERY_REQUIRED = "recovery_required" RECOVERY_EXHAUSTED = "recovery_exhausted" ROWS_DEACTIVATED = "rows_deactivated" SESSION_COMPLETED = "session_completed" @@ -350,6 +352,191 @@ def __post_init__(self) -> None: object.__setattr__(self, "failure_mask", self.failure_mask.clone()) +@dataclass(frozen=True, slots=True, eq=False) +class HeldObjectGuardRequest: + """Describe the next in-flight command boundary for held-object checks. + + A request is correlated to one installed action-plan attempt and one next + waypoint. The named segment lets an external verifier select phase-aware + physical evidence without teaching the execution core skill-specific + phases. ``deadline`` uses the observation timestamp domain. + """ + + verification_id: int + skill_id: str + invocation_id: str | None + invocation_revision: int + invocation_index: int + attempt_generation: int + next_waypoint_index: int + segment_name: str + env_mask: torch.Tensor + allowed_held_object_relations: tuple[tuple[str, str], ...] + allowed_coordinated_held_object_relations: tuple[tuple[str, str, str], ...] + deadline: float + + def __post_init__(self) -> None: + if type(self.verification_id) is not int or self.verification_id < 0: + raise ValueError("verification_id must be a non-negative integer.") + if type(self.skill_id) is not str or not self.skill_id: + raise ValueError("skill_id must be a non-empty string.") + if self.invocation_id is not None and ( + type(self.invocation_id) is not str or not self.invocation_id + ): + raise ValueError("invocation_id must be a non-empty string or None.") + for name in ( + "invocation_revision", + "invocation_index", + "attempt_generation", + "next_waypoint_index", + ): + value = getattr(self, name) + if type(value) is not int or value < 0: + raise ValueError(f"{name} must be a non-negative integer.") + if type(self.segment_name) is not str or not self.segment_name: + raise ValueError("segment_name must be a non-empty string.") + if not isinstance(self.env_mask, torch.Tensor): + raise TypeError("env_mask must be a torch.Tensor.") + if self.env_mask.dtype != torch.bool or self.env_mask.dim() != 1: + raise ValueError("env_mask must be a one-dimensional bool tensor.") + if not self.env_mask.any(): + raise ValueError("env_mask must contain at least one guarded row.") + held_relations = tuple(self.allowed_held_object_relations) + if len(set(held_relations)) != len(held_relations) or not all( + type(value) is tuple + and len(value) == 2 + and all(type(item) is str and item for item in value) + for value in held_relations + ): + raise ValueError( + "allowed_held_object_relations must contain unique " + "(task_state_key, object_id) pairs." + ) + coordinated_relations = tuple(self.allowed_coordinated_held_object_relations) + if len(set(coordinated_relations)) != len(coordinated_relations) or not all( + type(value) is tuple + and len(value) == 3 + and all(type(item) is str and item for item in value) + for value in coordinated_relations + ): + raise ValueError( + "allowed_coordinated_held_object_relations must contain unique " + "(first_key, second_key, object_id) triples." + ) + if not math.isfinite(self.deadline) or self.deadline < 0.0: + raise ValueError("deadline must be finite and non-negative.") + object.__setattr__(self, "env_mask", self.env_mask.clone()) + object.__setattr__(self, "allowed_held_object_relations", held_relations) + object.__setattr__( + self, + "allowed_coordinated_held_object_relations", + coordinated_relations, + ) + + def snapshot(self) -> HeldObjectGuardRequest: + """Return an independently owned guard request. + + Returns: + Request with an independently owned environment mask. + """ + return HeldObjectGuardRequest( + verification_id=self.verification_id, + skill_id=self.skill_id, + invocation_id=self.invocation_id, + invocation_revision=self.invocation_revision, + invocation_index=self.invocation_index, + attempt_generation=self.attempt_generation, + next_waypoint_index=self.next_waypoint_index, + segment_name=self.segment_name, + env_mask=self.env_mask, + allowed_held_object_relations=self.allowed_held_object_relations, + allowed_coordinated_held_object_relations=( + self.allowed_coordinated_held_object_relations + ), + deadline=self.deadline, + ) + + +@dataclass(frozen=True, slots=True, eq=False) +class HeldObjectGuardResult: + """Correlated in-flight held-object loss and recovery decision. + + ``state_invalidation`` may only remove single-resource or coordinated + held-object relations. It is applied to ``failure_mask`` before recovery + planning, so a retry always observes reconciled symbolic state. + """ + + verification_id: int + object_id: str + attempt_generation: int + invocation_index: int + next_waypoint_index: int + failure_mask: torch.Tensor + state_invalidation: StateDelta + retry_mask: torch.Tensor + message: str = "" + + def __post_init__(self) -> None: + if type(self.verification_id) is not int or self.verification_id < 0: + raise ValueError("verification_id must be a non-negative integer.") + if type(self.object_id) is not str or not self.object_id: + raise ValueError("object_id must be a non-empty string.") + for name in ( + "attempt_generation", + "invocation_index", + "next_waypoint_index", + ): + 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 ("failure_mask", "retry_mask"): + value = getattr(self, name) + if not isinstance(value, torch.Tensor): + raise TypeError(f"{name} must be a torch.Tensor.") + if value.dtype != torch.bool or value.dim() != 1: + raise ValueError(f"{name} must be a one-dimensional bool tensor.") + if self.failure_mask.shape != self.retry_mask.shape: + raise ValueError("failure_mask and retry_mask must have equal shapes.") + if self.failure_mask.device != self.retry_mask.device: + raise ValueError("failure_mask and retry_mask must use the same device.") + if (self.retry_mask & ~self.failure_mask).any(): + raise ValueError("retry_mask must be a subset of failure_mask.") + if not isinstance(self.state_invalidation, StateDelta): + raise TypeError("state_invalidation must be a StateDelta.") + if any( + value is not None + for value in self.state_invalidation.held_object_updates.values() + ) or any( + value is not None + for value in self.state_invalidation.coordinated_held_object_updates.values() + ): + raise ValueError( + "state_invalidation may only remove held-object relations." + ) + if self.state_invalidation.articulation_joint_updates: + raise ValueError( + "state_invalidation cannot update articulation-joint state." + ) + has_invalidation = bool( + self.state_invalidation.held_object_updates + or self.state_invalidation.coordinated_held_object_updates + ) + if bool(self.failure_mask.any().item()) != has_invalidation: + raise ValueError( + "state_invalidation must contain relation removals exactly when " + "failure_mask contains failed rows." + ) + if type(self.message) is not str: + raise TypeError("message must be a string.") + object.__setattr__(self, "failure_mask", self.failure_mask.clone()) + object.__setattr__(self, "retry_mask", self.retry_mask.clone()) + object.__setattr__( + self, + "state_invalidation", + self.state_invalidation.snapshot(), + ) + + @dataclass(frozen=True, slots=True, eq=False) class ExecutionTick: """Result returned after one closed-loop execution update.""" @@ -481,6 +668,7 @@ def __init__( self._effect_failures = torch.zeros_like(self._eligible) self._effect_requested_at: float | None = None self._next_effect_verification_id = 0 + self._next_held_object_guard_verification_id = 0 self._plan_attempt_records: list[_ExecutionPlanAttemptRecord] = [] self._status = ( ExecutionStatus.RUNNING if self._eligible.any() else ExecutionStatus.FAILED @@ -527,6 +715,22 @@ def pending_effect(self) -> EffectVerificationRequest | None: """Owned snapshot of the current effect boundary, when present.""" return None if self._pending_effect is None else self._pending_effect.snapshot() + @property + def held_object_guard_request(self) -> HeldObjectGuardRequest | None: + """Describe the phase that must be checked before the next command. + + The request remains available while terminal acceptance is settling, + using the final waypoint and segment identity. Once terminal physical + effect verification begins, that verifier owns the boundary and this + property returns ``None``. + + Returns: + Owned phase-aware guard request, or ``None`` when no command-phase + guard is active. + """ + request = self._held_object_guard_request() + return None if request is None else request.snapshot() + def deactivate_rows( self, env_mask: torch.Tensor, @@ -764,6 +968,7 @@ def tick( context: PlanningContext, *, effect_result: EffectVerificationResult | None = None, + held_object_guard_result: HeldObjectGuardResult | None = None, ) -> ExecutionTick: """Advance execution by one observation/command cycle. @@ -772,6 +977,10 @@ def tick( state is replaced by the session's verified task state. effect_result: Optional correlated semantic-effect result for an action waiting at its terminal waypoint. + held_object_guard_result: Optional correlated in-flight held-object + loss result for the current waypoint phase. ``None`` means the + verifier found no applicable guard for this phase or no result + was supplied. Returns: Status, optional command, events, and current verified task state. @@ -790,10 +999,52 @@ def tick( "effect_result verification_id does not match the pending " "effect boundary." ) + guard_request = self._held_object_guard_request() + if held_object_guard_result is not None: + if type(held_object_guard_result) is not HeldObjectGuardResult: + raise TypeError( + "held_object_guard_result must be exactly " + "HeldObjectGuardResult or None." + ) + if guard_request is None: + raise ValueError("No held-object guard is active for this phase.") + if held_object_guard_result.verification_id != ( + guard_request.verification_id + ): + raise ValueError( + "held_object_guard_result verification_id does not match the " + "active guard request." + ) + for name in ( + "attempt_generation", + "invocation_index", + "next_waypoint_index", + ): + if getattr(held_object_guard_result, name) != getattr( + guard_request, + name, + ): + raise ValueError( + f"held_object_guard_result {name} does not match the " + "active guard request." + ) + if guard_request is not None: + self._next_held_object_guard_verification_id += 1 if self._status is not ExecutionStatus.RUNNING: return self._tick_result(command=None, events=events) assert self._plan is not None + if held_object_guard_result is not None: + assert guard_request is not None + events.extend( + self._apply_held_object_guard_result( + held_object_guard_result, + guard_request, + ) + ) + if self._status is not ExecutionStatus.RUNNING: + return self._tick_result(command=None, events=events) + assert self._plan is not None if not self._pending.any(): command, hold_targets, completion_events = self._finish_action( self._pending, @@ -1845,6 +2096,200 @@ def _batched_entity_pose(self, state: EntityState) -> torch.Tensor: raise ValueError("Scene entity pose batch does not match the session.") return pose + def _held_object_guard_request(self) -> HeldObjectGuardRequest | None: + """Build the current command-phase held-object guard request.""" + if ( + self._status is not ExecutionStatus.RUNNING + or self._plan is None + or self._pending_effect is not None + or self._effect_failures.any() + or self._plan.commands.frame_count == 0 + ): + return None + env_mask = self._pending & self._plan.plan_success + if not env_mask.any(): + return None + next_waypoint_index = min( + self._waypoint_index, + self._plan.commands.frame_count - 1, + ) + segment = self._plan.segment_at(next_waypoint_index) + invocation = self._requests[self._invocation_index] + ( + allowed_held_object_relations, + allowed_coordinated_held_object_relations, + ) = self._authorized_held_object_invalidation_relations( + invocation=invocation, + ) + return HeldObjectGuardRequest( + verification_id=self._next_held_object_guard_verification_id, + skill_id=invocation.skill_id, + invocation_id=invocation.invocation_id, + invocation_revision=invocation.revision, + invocation_index=self._invocation_index, + attempt_generation=self._attempt_generation, + next_waypoint_index=next_waypoint_index, + segment_name=segment.name, + env_mask=env_mask, + allowed_held_object_relations=allowed_held_object_relations, + allowed_coordinated_held_object_relations=( + allowed_coordinated_held_object_relations + ), + deadline=( + self._action_started_at + self._plan.recovery_policy.action_timeout + ), + ) + + def _apply_held_object_guard_result( + self, + result: HeldObjectGuardResult, + request: HeldObjectGuardRequest, + ) -> list[ExecutionEvent]: + """Reconcile lost relations and enter row-local bounded recovery.""" + failure_mask = self._normalize_mask( + result.failure_mask, + "held_object_guard_result.failure_mask", + ) + retry_mask = self._normalize_mask( + result.retry_mask, + "held_object_guard_result.retry_mask", + ) + request_mask = request.env_mask.to(self._eligible.device) + if (failure_mask & ~request_mask).any(): + raise ValueError( + "Held-object guard failure_mask must be a subset of the active " + "request env_mask." + ) + if (retry_mask & ~request_mask).any(): + raise ValueError( + "Held-object guard retry_mask must be a subset of the active " + "request env_mask." + ) + self._validate_held_object_invalidation_authorization( + result.state_invalidation, + object_id=result.object_id, + allowed_held_object_relations=request.allowed_held_object_relations, + allowed_coordinated_held_object_relations=( + request.allowed_coordinated_held_object_relations + ), + ) + if not failure_mask.any(): + return [] + + self._task_state = result.state_invalidation.apply( + self._task_state, + failure_mask, + ) + self._context = PlanningContext( + robot=self._context.robot, + task=self._task_state, + scene=self._context.scene, + env_ids=self._context.env_ids, + ) + message = result.message or ( + "Physical evidence contradicted the verified held-object relation." + ) + non_retry_mask = failure_mask & ~retry_mask + events: list[ExecutionEvent] = [] + if non_retry_mask.any(): + self._eligible &= ~non_retry_mask + self._pending &= ~non_retry_mask + self._effect_failures &= ~non_retry_mask + self._last_command_mask &= ~non_retry_mask + events.extend( + ( + self._event( + ExecutionEventKind.HELD_OBJECT_LOST, + non_retry_mask, + message, + ), + self._event( + ExecutionEventKind.RECOVERY_REQUIRED, + non_retry_mask, + "Held-object loss requires recovery outside the current " + "action retry policy.", + ), + ) + ) + if retry_mask.any(): + events.extend( + self._attempt_action_retry( + retry_mask, + ExecutionEventKind.HELD_OBJECT_LOST, + message, + ) + ) + else: + terminal_event = self._update_terminal_status() + if terminal_event is not None: + events.append(terminal_event) + return events + + def _authorized_held_object_invalidation_relations( + self, + *, + invocation: ResolvedActionRequest | None = None, + ) -> tuple[tuple[tuple[str, str], ...], tuple[tuple[str, str, str], ...]]: + """Return action-owned key/object identities eligible for removal.""" + assert self._plan is not None + active_invocation = ( + self._requests[self._invocation_index] if invocation is None else invocation + ) + binding_task_state_keys = { + endpoint.task_state_key for endpoint in active_invocation.binding.endpoints + } + held_relations: set[tuple[str, str]] = set() + for key, candidate in self._task_state.held_objects.items(): + object_id = candidate.semantics.entity_id + if key in binding_task_state_keys and object_id is not None: + held_relations.add((key, object_id)) + for key, candidate in self._plan.expected_effects.held_object_updates.items(): + if candidate is not None and candidate.semantics.entity_id is not None: + held_relations.add((key, candidate.semantics.entity_id)) + + related_keys = {key for key, _ in held_relations} + coordinated_relations: set[tuple[str, str, str]] = set() + for resources, candidate in self._task_state.coordinated_held_objects.items(): + object_id = candidate.semantics.entity_id + if not set(resources).isdisjoint(related_keys) and object_id is not None: + coordinated_relations.add((*resources, object_id)) + for ( + resources, + candidate, + ) in self._plan.expected_effects.coordinated_held_object_updates.items(): + if candidate is not None and candidate.semantics.entity_id is not None: + coordinated_relations.add((*resources, candidate.semantics.entity_id)) + return tuple(sorted(held_relations)), tuple(sorted(coordinated_relations)) + + def _validate_held_object_invalidation_authorization( + self, + state_invalidation: StateDelta, + *, + object_id: str, + allowed_held_object_relations: tuple[tuple[str, str], ...], + allowed_coordinated_held_object_relations: tuple[tuple[str, str, str], ...], + ) -> None: + """Reject removals outside the action-owned key/object identity set.""" + invalidated_held_relations = { + (key, object_id) for key in state_invalidation.held_object_updates + } + if not invalidated_held_relations.issubset(allowed_held_object_relations): + raise ValueError( + "Held-object state invalidation contains a key/object identity " + "outside the active action's authorized relation set." + ) + invalidated_coordinated_relations = { + (*resources, object_id) + for resources in state_invalidation.coordinated_held_object_updates + } + if not invalidated_coordinated_relations.issubset( + allowed_coordinated_held_object_relations + ): + raise ValueError( + "Held-object state invalidation contains a coordinated key/object " + "identity outside the active action's authorized relation set." + ) + def _normalize_mask(self, value: torch.Tensor, name: str) -> torch.Tensor: """Validate and copy a per-environment boolean mask.""" if not isinstance(value, torch.Tensor): @@ -1966,4 +2411,6 @@ def _tick_result( "ExecutionSession", "ExecutionStatus", "ExecutionTick", + "HeldObjectGuardRequest", + "HeldObjectGuardResult", ] diff --git a/embodichain/lab/sim/atomic_actions/runner.py b/embodichain/lab/sim/atomic_actions/runner.py index 8dac661a6..0c7601fad 100644 --- a/embodichain/lab/sim/atomic_actions/runner.py +++ b/embodichain/lab/sim/atomic_actions/runner.py @@ -36,6 +36,8 @@ ExecutionSession, ExecutionStatus, ExecutionTick, + HeldObjectGuardRequest, + HeldObjectGuardResult, ) from .invocation import ActionInvocation, ResolvedActionRequest from .runtime_commands import RuntimeCommandFrame @@ -236,6 +238,15 @@ class ExecutionRunnerCfg: hold_on_completion: bool = True """Whether to issue a final hold after the session completes.""" + hold_during_effect_verification: bool = True + """Whether to hold observed state while terminal effects are pending. + + Disable this only for persistent transports whose last accepted command + remains active without refresh, such as a position-controlled gripper that + must retain contact preload. Failure and cancellation still perform the + normal cancel-then-observed-hold safe stop. + """ + def __post_init__(self) -> None: for name in ("command_timeout", "safe_stop_timeout"): value = getattr(self, name) @@ -245,6 +256,8 @@ def __post_init__(self) -> None: raise ValueError("minimum_cycle_time must be finite and non-negative.") if not isinstance(self.hold_on_completion, bool): raise TypeError("hold_on_completion must be a bool.") + if not isinstance(self.hold_during_effect_verification, bool): + raise TypeError("hold_during_effect_verification must be a bool.") class RunnerStatus(str, Enum): @@ -299,6 +312,12 @@ def is_waiting(self) -> bool: ] """Synchronous verifier called on a fresh due-cycle observation.""" +HeldObjectGuardVerifier = Callable[ + [PlanningContext, HeldObjectGuardRequest], + HeldObjectGuardResult | None, +] +"""Synchronous phase-aware held-object verifier for one due command cycle.""" + RunnerStepCallback = Callable[[RunnerStep], None] """Optional observer called after every blocking runner-loop iteration.""" @@ -466,6 +485,7 @@ def step( *, effect_result: EffectVerificationResult | None = None, effect_verifier: EffectVerifier | None = None, + held_object_guard_verifier: HeldObjectGuardVerifier | None = None, ) -> RunnerStep: """Perform one due observation/session/controller update without sleeping. @@ -478,6 +498,11 @@ def step( and before the session consumes the result. It is not called after the request deadline. Mutually exclusive with ``effect_result``. + held_object_guard_verifier: Optional synchronous phase-aware + verifier. It receives a fresh observation and the current + command-phase request before :meth:`ExecutionSession.tick` and + command dispatch. Returning ``None`` means the current phase + has no applicable held-object guard. Returns: Runner status, optional session tick, controller acknowledgements, @@ -489,6 +514,10 @@ def step( ) if effect_verifier is not None and not callable(effect_verifier): raise TypeError("effect_verifier must be callable or None.") + if held_object_guard_verifier is not None and not callable( + held_object_guard_verifier + ): + raise TypeError("held_object_guard_verifier must be callable or None.") now = self._clock_now() if self._status is not RunnerStatus.RUNNING: return self._result(timestamp=now) @@ -512,6 +541,19 @@ def step( ) self._last_context = context + try: + if self._pending_revision is not None: + self._session._install_prepared_revision( + self._pending_revision, + context, + ) + self._pending_revision = None + except Exception as exc: + return self._fail( + f"Execution session failed: {type(exc).__name__}: {exc}", + context=context, + ) + pending_effect = self._session.pending_effect if ( effect_verifier is not None @@ -531,14 +573,39 @@ def step( context=context, ) - try: - if self._pending_revision is not None: - self._session._install_prepared_revision( - self._pending_revision, + held_object_guard_result: HeldObjectGuardResult | None = None + held_object_guard_request = self._session.held_object_guard_request + if ( + held_object_guard_verifier is not None + and held_object_guard_request is not None + and context.robot.timestamp <= held_object_guard_request.deadline + ): + try: + held_object_guard_result = held_object_guard_verifier( context, + held_object_guard_request, ) - self._pending_revision = None - tick = self._session.tick(context, effect_result=effect_result) + if ( + held_object_guard_result is not None + and type(held_object_guard_result) is not HeldObjectGuardResult + ): + raise TypeError( + "HeldObjectGuardVerifier must return exactly " + "HeldObjectGuardResult or None." + ) + except Exception as exc: + return self._fail( + "Held-object guard verifier failed: " + f"{type(exc).__name__}: {exc}", + context=context, + ) + + try: + tick = self._session.tick( + context, + effect_result=effect_result, + held_object_guard_result=held_object_guard_result, + ) context = self._session.latest_context self._last_context = context except Exception as exc: @@ -581,7 +648,9 @@ def step( self._command_count += 1 interval = self._command_interval(tick.command) self._next_step_at = self._clock_now() + interval - elif tick.hold_targets: + elif tick.hold_targets and ( + tick.pending_effect is None or self.cfg.hold_during_effect_verification + ): self._remember_targets(tick.hold_targets) hold_dispatch = self._dispatch( CommandOperation.HOLD, @@ -605,6 +674,7 @@ def step( ) self._next_step_at = self._clock_now() + self.cfg.minimum_cycle_time elif tick.pending_effect is not None: + self._remember_targets(tick.hold_targets) self._next_step_at = self._clock_now() + self.cfg.minimum_cycle_time else: self._next_step_at = self._clock_now() @@ -685,6 +755,7 @@ def run_until_blocked( self, *, effect_verifier: EffectVerifier | None = None, + held_object_guard_verifier: HeldObjectGuardVerifier | None = None, on_step: RunnerStepCallback | None = None, max_steps: int = 100_000, ) -> RunnerStep: @@ -695,6 +766,8 @@ def run_until_blocked( due-cycle observations while effect verification is pending. Without one, the method returns the running boundary so the caller can verify externally. + held_object_guard_verifier: Optional synchronous phase-aware + held-object verifier used before every due command cycle. on_step: Optional callback for tracing or tutorial visualization. max_steps: Hard bound on loop iterations. @@ -713,7 +786,10 @@ def run_until_blocked( if self.effect_verification_pending and effect_verifier is None: return last_result for _ in range(max_steps): - result = self.step(effect_verifier=effect_verifier) + result = self.step( + effect_verifier=effect_verifier, + held_object_guard_verifier=held_object_guard_verifier, + ) if on_step is not None: try: on_step(result) @@ -925,6 +1001,7 @@ def _result( "ExecutionClock", "ExecutionRunner", "ExecutionRunnerCfg", + "HeldObjectGuardVerifier", "MonotonicExecutionClock", "ObservationProvider", "RunnerStatus", diff --git a/embodichain/lab/sim/skills/__init__.py b/embodichain/lab/sim/skills/__init__.py index 9bd87c54a..95381c478 100644 --- a/embodichain/lab/sim/skills/__init__.py +++ b/embodichain/lab/sim/skills/__init__.py @@ -34,9 +34,11 @@ ) from .compiler import ( AnalyzedSemanticCall, + GroundedHeldObjectGuard, GroundedSemanticCall, HandOverPoseProvider, HandOverPoseTargets, + HeldObjectGuardBaseline, RegisteredSemanticLowerer, RelationTargetGrounder, SemanticEffectDependency, @@ -267,12 +269,14 @@ "EffectStateExpectation", "FORCE_EFFECT_CHANNEL", "GRASP_AFFORDANCE_CAPABILITY", + "GroundedHeldObjectGuard", "GroundedSemanticCall", "HeldObjectRelation", "HeldObjectStateExpectation", "HandOver", "HandOverPoseProvider", "HandOverPoseTargets", + "HeldObjectGuardBaseline", "LinkedSemanticCall", "JOINT_STATE_EFFECT_CHANNEL", "JointStateEffectClause", diff --git a/embodichain/lab/sim/skills/compiler.py b/embodichain/lab/sim/skills/compiler.py index 98040c412..4b23d3bca 100644 --- a/embodichain/lab/sim/skills/compiler.py +++ b/embodichain/lab/sim/skills/compiler.py @@ -22,6 +22,7 @@ from collections.abc import Iterable, Mapping from copy import deepcopy from dataclasses import dataclass, field, replace +from enum import Enum from types import MappingProxyType from typing import ClassVar, TypeVar from uuid import uuid4 @@ -77,6 +78,8 @@ JointStateEffectClause, PoseRelationClause, PoseRelationExpectation, + ScalarEffectClause, + ScalarExpectation, SemanticEffectKind, SemanticEffectSpec, SymbolicStateKey, @@ -475,6 +478,85 @@ def resolve( """ +class HeldObjectGuardBaseline(str, Enum): + """Source of the verified pose baseline used by an in-flight guard.""" + + VERIFIED_TASK_STATE = "verified_task_state" + PLANNED_EFFECT = "planned_effect" + + +@dataclass(frozen=True, slots=True) +class GroundedHeldObjectGuard: + """One grounded, phase-scoped physical invariant for a held object. + + Named trajectory segments only activate observation of the invariant; they + do not create an independent planning, timeout, or recovery boundary. The + enclosing atomic action continues to own the recovery budget. + """ + + guard_id: str + active_segments: tuple[str, ...] + baseline: HeldObjectGuardBaseline + effect_spec: SemanticEffectSpec + effect_monitor: EffectMonitor = field(repr=False, compare=False) + invalidation_task_state_keys: tuple[str, ...] + retry_action: bool + + def __post_init__(self) -> None: + _validate_identifier(self.guard_id, field_name="guard_id") + segments = tuple(self.active_segments) + if not segments or len(set(segments)) != len(segments): + raise ValueError( + "active_segments must contain unique non-empty segment names." + ) + for segment in segments: + _validate_identifier(segment, field_name="active segment") + if not isinstance(self.baseline, HeldObjectGuardBaseline): + raise TypeError("baseline must be a HeldObjectGuardBaseline.") + if not isinstance(self.effect_spec, SemanticEffectSpec): + raise TypeError("effect_spec must be a SemanticEffectSpec.") + if self.effect_spec.effect_kind is not SemanticEffectKind.ATTACH: + raise ValueError("A held-object guard must observe an attach effect.") + expectations = tuple( + value + for value in self.effect_spec.state_expectations + if type(value) is HeldObjectStateExpectation + ) + if len(expectations) != 1 or expectations[0].relation is not ( + HeldObjectRelation.ATTACHED + ): + raise ValueError( + "A held-object guard must contain one attached expectation." + ) + if not isinstance(self.effect_monitor, EffectMonitor): + raise TypeError("effect_monitor must be an EffectMonitor.") + invalidation_keys = tuple(self.invalidation_task_state_keys) + if not invalidation_keys or len(set(invalidation_keys)) != len( + invalidation_keys + ): + raise ValueError( + "invalidation_task_state_keys must contain unique non-empty keys." + ) + for key in invalidation_keys: + _validate_identifier(key, field_name="invalidation task-state key") + if type(self.retry_action) is not bool: + raise TypeError("retry_action must be a bool.") + object.__setattr__(self, "active_segments", segments) + object.__setattr__(self, "effect_spec", self.effect_spec.snapshot()) + object.__setattr__( + self, + "invalidation_task_state_keys", + invalidation_keys, + ) + + @property + def task_state_key(self) -> str: + """Return the single held-object relation observed by this guard.""" + expectation = self.effect_spec.state_expectations[0] + assert type(expectation) is HeldObjectStateExpectation + return expectation.task_state_key + + @dataclass(frozen=True, slots=True, init=False) class GroundedSemanticCall: """Factory-owned call lowered from the latest observed context.""" @@ -483,6 +565,7 @@ class GroundedSemanticCall: invocation: ActionInvocation effect_spec: SemanticEffectSpec | None effect_monitor: EffectMonitor | None = field(repr=False, compare=False) + effect_guards: tuple[GroundedHeldObjectGuard, ...] _eligible_mask: torch.Tensor = field(repr=False, compare=False) def __init__(self, *args: object, **kwargs: object) -> None: @@ -501,6 +584,7 @@ def _create( invocation: ActionInvocation, effect_spec: SemanticEffectSpec | None, effect_monitor: EffectMonitor | None, + effect_guards: tuple[GroundedHeldObjectGuard, ...], eligible_mask: torch.Tensor, ) -> GroundedSemanticCall: """Create one compiler-owned grounded result.""" @@ -509,6 +593,7 @@ def _create( object.__setattr__(instance, "invocation", invocation) object.__setattr__(instance, "effect_spec", effect_spec) object.__setattr__(instance, "effect_monitor", effect_monitor) + object.__setattr__(instance, "effect_guards", tuple(effect_guards)) object.__setattr__(instance, "_eligible_mask", eligible_mask.clone()) instance.__post_init__() return instance @@ -534,6 +619,17 @@ def __post_init__(self) -> None: "effect_spec semantic_id must match the analyzed call." ) object.__setattr__(self, "effect_spec", self.effect_spec.snapshot()) + guards = tuple(self.effect_guards) + if not all(type(value) is GroundedHeldObjectGuard for value in guards): + raise TypeError( + "effect_guards must contain exact GroundedHeldObjectGuard values." + ) + guard_ids = [value.guard_id for value in guards] + if len(set(guard_ids)) != len(guard_ids): + raise ValueError("Grounded held-object guard IDs must be unique.") + if guards and self.effect_spec is None: + raise ValueError("Held-object guards require a terminal effect spec.") + object.__setattr__(self, "effect_guards", guards) if not isinstance(self._eligible_mask, torch.Tensor): raise TypeError("eligible_mask must be a torch.Tensor.") if self._eligible_mask.dtype != torch.bool or self._eligible_mask.dim() != 1: @@ -1124,11 +1220,18 @@ def ground( (*path, call_index, "effect_monitor"), f"Could not create the grounded effect monitor: {exc}", ) from exc + effect_guards = self._ground_held_object_guards( + analyzed, + effect_spec, + context, + path=(*path, call_index, "effect_guards"), + ) return GroundedSemanticCall._create( analyzed=analyzed, invocation=invocation, effect_spec=effect_spec, effect_monitor=effect_monitor, + effect_guards=effect_guards, eligible_mask=eligible, ) @@ -1761,6 +1864,209 @@ def _ground_effect_spec( clauses=tuple(clauses), ) + def _ground_held_object_guards( + self, + analyzed: AnalyzedSemanticCall, + effect_spec: SemanticEffectSpec | None, + context: PlanningContext, + *, + path: tuple[PathPart, ...], + ) -> tuple[GroundedHeldObjectGuard, ...]: + """Create phase-scoped held-object invariants for built-in semantics. + + The guard observes only named action segments whose commanded motion + assumes that a particular endpoint still holds the object. It never + creates or repairs a physical relation. + + Args: + analyzed: Statically linked semantic call. + effect_spec: Grounded terminal effect contract for the call. + context: Latest planning context used to validate verified baselines. + path: Diagnostic path for monitor-construction failures. + + Returns: + Independent guard monitors in deterministic phase order. + """ + monitor_ref = analyzed.effect_monitor_ref + if effect_spec is None or monitor_ref is None: + return () + + call = analyzed.call + definitions: tuple[ + tuple[ + str, + str, + tuple[str, ...], + HeldObjectGuardBaseline, + tuple[str, ...], + bool, + ], + ..., + ] + if type(call) is Pick: + destination = self._held_expectation(effect_spec, "destination") + definitions = ( + ( + "destination_attached", + destination.expectation_id, + ("lift",), + HeldObjectGuardBaseline.PLANNED_EFFECT, + (destination.task_state_key,), + True, + ), + ) + elif type(call) is Place: + source = self._held_expectation(effect_spec, "source") + self._validate_guard_verified_baseline(source, context) + definitions = ( + ( + "source_attached", + source.expectation_id, + ("approach",), + HeldObjectGuardBaseline.VERIFIED_TASK_STATE, + (source.task_state_key,), + False, + ), + ) + elif type(call) is HandOver: + source = self._held_expectation(effect_spec, "source") + destination = self._held_expectation(effect_spec, "destination") + self._validate_guard_verified_baseline(source, context) + definitions = ( + ( + "source_attached", + source.expectation_id, + ("transfer", "approach", "close", "hold"), + HeldObjectGuardBaseline.VERIFIED_TASK_STATE, + (source.task_state_key,), + False, + ), + ( + "destination_attached", + destination.expectation_id, + ("release", "deliver"), + HeldObjectGuardBaseline.PLANNED_EFFECT, + (source.task_state_key, destination.task_state_key), + False, + ), + ) + else: + return () + + guards: list[GroundedHeldObjectGuard] = [] + for ( + guard_id, + expectation_id, + active_segments, + baseline, + invalidation_keys, + retry_action, + ) in definitions: + guard_spec = self._attached_guard_effect_spec( + effect_spec, + expectation_id=expectation_id, + ) + try: + monitor = self._effect_monitor_registry.create( + guard_spec, + monitor_ref, + ) + except (KeyError, TypeError, ValueError) as exc: + raise _diagnostic( + "effect_guard_monitor_creation_failed", + (*path, guard_id), + f"Could not create held-object guard monitor: {exc}", + ) from exc + guards.append( + GroundedHeldObjectGuard( + guard_id=guard_id, + active_segments=active_segments, + baseline=baseline, + effect_spec=guard_spec, + effect_monitor=monitor, + invalidation_task_state_keys=invalidation_keys, + retry_action=retry_action, + ) + ) + return tuple(guards) + + @staticmethod + def _held_expectation( + spec: SemanticEffectSpec, + expectation_id: str, + ) -> HeldObjectStateExpectation: + """Resolve one exact held-object expectation from an effect spec.""" + expectation = spec.state_expectation(expectation_id) + if type(expectation) is not HeldObjectStateExpectation: + raise ValueError( + f"Effect expectation {expectation_id!r} is not held-object state." + ) + return expectation + + @staticmethod + def _validate_guard_verified_baseline( + expectation: HeldObjectStateExpectation, + context: PlanningContext, + ) -> None: + """Require the task state to own the guard's verified relation.""" + held = context.task.get_held_object(expectation.task_state_key) + if held is None or held.semantics.entity_id != expectation.object_id: + raise ValueError( + f"Held-object guard {expectation.expectation_id!r} requires " + f"verified object {expectation.object_id!r} under task-state key " + f"{expectation.task_state_key!r}." + ) + + @staticmethod + def _attached_guard_effect_spec( + terminal_spec: SemanticEffectSpec, + *, + expectation_id: str, + ) -> SemanticEffectSpec: + """Project one terminal expectation into an attached invariant.""" + terminal_expectation = terminal_spec.state_expectation(expectation_id) + if type(terminal_expectation) is not HeldObjectStateExpectation: + raise TypeError("Held-object guards require held-object expectations.") + attached = replace( + terminal_expectation, + relation=HeldObjectRelation.ATTACHED, + ) + clauses: list[EffectClause] = [] + for clause in terminal_spec.clauses: + if clause.expectation_id != expectation_id: + continue + if type(clause) is PoseRelationClause: + clauses.append( + PoseRelationClause( + clause_id=clause.clause_id, + expectation_id=clause.expectation_id, + source=clause.source, + expectation=PoseRelationExpectation.MATCHED, + ) + ) + elif type(clause) is BinaryEffectClause: + clauses.append(replace(clause, expected=True)) + elif type(clause) is ScalarEffectClause: + clauses.append(replace(clause, expectation=ScalarExpectation.PRESENT)) + else: + raise TypeError( + "Held-object guards support pose, binary, and scalar clauses." + ) + if not clauses: + raise ValueError( + f"Held-object expectation {expectation_id!r} has no physical clauses." + ) + return SemanticEffectSpec( + semantic_id=terminal_spec.semantic_id, + effect_kind=SemanticEffectKind.ATTACH, + skill_id=terminal_spec.skill_id, + invocation_id=terminal_spec.invocation_id, + invocation_revision=terminal_spec.invocation_revision, + env_ids=terminal_spec.env_ids, + state_expectations=(attached,), + clauses=tuple(clauses), + ) + @staticmethod def _coordinated_cleanup_expectations( context: PlanningContext, @@ -2180,9 +2486,11 @@ def _broadcast_joint_position( __all__ = [ "AnalyzedSemanticCall", + "GroundedHeldObjectGuard", "GroundedSemanticCall", "HandOverPoseProvider", "HandOverPoseTargets", + "HeldObjectGuardBaseline", "RelationTargetGrounder", "RegisteredSemanticLowerer", "SemanticEffectDependency", diff --git a/embodichain/lab/sim/skills/runtime.py b/embodichain/lab/sim/skills/runtime.py index 46fe7da6e..b9d6bd96a 100644 --- a/embodichain/lab/sim/skills/runtime.py +++ b/embodichain/lab/sim/skills/runtime.py @@ -29,11 +29,14 @@ from ..atomic_actions.bindings import EndpointBinding from ..atomic_actions.engine import AtomicActionEngine +from ..atomic_actions.effects import StateDelta from ..atomic_actions.execution import ( EffectVerificationRequest, EffectVerificationResult, ExecutionEvent, ExecutionPlanAttempt, + HeldObjectGuardRequest, + HeldObjectGuardResult, ) from ..atomic_actions.plans import TrajectorySegment from ..atomic_actions.policies import MotionPolicy, RecoveryPolicy @@ -47,7 +50,7 @@ RunnerStatus, RunnerStep, ) -from ..atomic_actions.state import PlanningContext, TaskState +from ..atomic_actions.state import HeldObjectState, PlanningContext, TaskState from ..atomic_actions.tracking import ( FeedbackTerminalAcceptance, TimedTrackingSequence, @@ -55,11 +58,16 @@ TrackingPolicy, ) from .calls import SemanticCallSpec -from .compiler import SemanticSkillCompiler +from .compiler import ( + GroundedHeldObjectGuard, + HeldObjectGuardBaseline, + SemanticSkillCompiler, +) from .effects import ( BinaryEffectEvidenceBatch, EffectEvidenceBatch, EffectMonitor, + EffectMonitorDecision, EffectMonitorRef, JointStateEvidenceBatch, PoseRelationEvidenceBatch, @@ -953,6 +961,9 @@ class SkillEffectTrace: configured_monitor_params: Mapping[str, object] resolved_monitor_params: Mapping[str, object] evidence: Mapping[str, EffectEvidenceBatch] + boundary_kind: str = "terminal" + guard_id: str | None = None + segment_name: str | None = None def __post_init__(self) -> None: if type(self.call_index) is not int or self.call_index < 0: @@ -961,6 +972,21 @@ def __post_init__(self) -> None: raise ValueError("verification_id must be a non-negative integer.") if type(self.observation_revision) is not int or self.observation_revision < 0: raise ValueError("observation_revision must be non-negative.") + if self.boundary_kind not in {"terminal", "in_flight_guard"}: + raise ValueError("boundary_kind must be 'terminal' or 'in_flight_guard'.") + for name in ("guard_id", "segment_name"): + value = getattr(self, name) + if value is not None and (type(value) is not str or not value): + raise ValueError(f"{name} must be a non-empty string or None.") + if self.boundary_kind == "terminal": + if self.guard_id is not None or self.segment_name is not None: + raise ValueError( + "Terminal effect traces cannot declare guard phase metadata." + ) + elif self.guard_id is None or self.segment_name is None: + raise ValueError( + "In-flight guard traces require guard_id and segment_name." + ) if not math.isfinite(self.timestamp) or self.timestamp < 0.0: raise ValueError("timestamp must be finite and non-negative.") for name in ("success_mask", "failure_mask"): @@ -1028,11 +1054,14 @@ def snapshot(self) -> SkillEffectTrace: configured_monitor_params=self.configured_monitor_params, resolved_monitor_params=self.resolved_monitor_params, evidence=self.evidence, + boundary_kind=self.boundary_kind, + guard_id=self.guard_id, + segment_name=self.segment_name, ) def to_metadata(self) -> dict[str, object]: """Return monitor contract, evidence, thresholds, and decision metadata.""" - return { + metadata = { "call_index": self.call_index, "verification_id": self.verification_id, "observation_revision": self.observation_revision, @@ -1053,6 +1082,15 @@ def to_metadata(self) -> dict[str, object]: "failure_mask": _metadata_value(self.failure_mask), }, } + metadata["boundary"] = {"kind": self.boundary_kind} + if self.boundary_kind == "in_flight_guard": + metadata["boundary"].update( + { + "guard_id": self.guard_id, + "segment_name": self.segment_name, + } + ) + return metadata @dataclass(frozen=True, slots=True, eq=False) @@ -1539,6 +1577,7 @@ def __init__( self._call_event_offset = 0 self._call_effect_offset = 0 self._observation_revision = 0 + self._next_guard_verification_id = 0 self._wait_duration = 0.0 self._message: str | None = None @@ -1704,7 +1743,12 @@ def step(self) -> SkillResult: grounded = self._require_grounded() monitor = getattr(grounded, "effect_monitor", None) verifier = self._effect_verifier if monitor is not None else None - runner_step = runner.step(effect_verifier=verifier) + guards = tuple(getattr(grounded, "effect_guards", ())) + guard_verifier = self._held_object_guard_verifier if guards else None + runner_step = runner.step( + effect_verifier=verifier, + held_object_guard_verifier=guard_verifier, + ) self._consume_runner_step(runner_step) if ( runner_step.status is RunnerStatus.RUNNING @@ -1970,6 +2014,7 @@ def _reset_workflow( self._call_event_offset = 0 self._call_effect_offset = 0 self._observation_revision = 0 + self._next_guard_verification_id = 0 self._wait_duration = 0.0 self._message = None self._status = SkillStatus.RUNNING @@ -2020,6 +2065,7 @@ def _prepare_call(self, call_index: int) -> None: grounded_eligible = getattr(grounded, "eligible_mask", None) effect_spec = getattr(grounded, "effect_spec", None) effect_monitor = getattr(grounded, "effect_monitor", None) + effect_guards = tuple(getattr(grounded, "effect_guards", ())) if invocation is None: raise TypeError("Semantic compiler ground() must return an invocation.") if not isinstance(grounded_eligible, torch.Tensor) or not torch.equal( @@ -2041,6 +2087,13 @@ def _prepare_call(self, call_index: int) -> None: context.env_ids, ): raise ValueError("Grounded effect env_ids must match the call context.") + if not all(type(value) is GroundedHeldObjectGuard for value in effect_guards): + raise TypeError( + "Grounded effect_guards must contain exact " + "GroundedHeldObjectGuard values." + ) + if effect_guards and effect_spec is None: + raise ValueError("Grounded held-object guards require an effect spec.") self._grounded = grounded session = self._engine.start( @@ -2088,6 +2141,166 @@ def _effect_verifier( ) if request.invocation_revision != spec.invocation_revision: raise ValueError("Effect request revision does not match the effect spec.") + decision = self._observe_effect_monitor( + context, + request, + spec=spec, + monitor=monitor, + ) + return EffectVerificationResult( + verification_id=request.verification_id, + success_mask=decision.success_mask, + failure_mask=decision.failure_mask, + ) + + def _held_object_guard_verifier( + self, + context: PlanningContext, + request: HeldObjectGuardRequest, + ) -> HeldObjectGuardResult | None: + """Observe a phase-scoped held-object invariant before dispatch. + + Args: + context: Fresh due-cycle physical observation. + request: Core-owned phase and correlation identity. + + Returns: + Correlated row-local loss decision, or ``None`` when this named + action segment has no held-object invariant. + """ + if context.robot.timestamp > request.deadline: + return None + grounded = self._require_grounded() + guards = tuple(getattr(grounded, "effect_guards", ())) + active = tuple( + guard for guard in guards if request.segment_name in guard.active_segments + ) + if not active: + return None + if len(active) != 1: + raise RuntimeError( + "At most one held-object guard may own an action segment; " + f"segment={request.segment_name!r}, guards=" + f"{[guard.guard_id for guard in active]}." + ) + guard = active[0] + session = self._require_runner().session + if guard.baseline is HeldObjectGuardBaseline.VERIFIED_TASK_STATE: + candidate = session.task_state.get_held_object(guard.task_state_key) + else: + candidate = session.active_plan.expected_effects.held_object_updates.get( + guard.task_state_key + ) + covered = torch.zeros_like(request.env_mask) + if isinstance(candidate, HeldObjectState): + covered = ( + torch.ones_like(request.env_mask) + if candidate.env_mask is None + else candidate.env_mask.to(request.env_mask.device) + ) + if candidate.semantics.entity_id != self._guard_object_id( + guard.effect_spec + ): + covered.zero_() + observed_mask = request.env_mask & covered + failure_mask = request.env_mask & ~covered + if observed_mask.any(): + assert isinstance(candidate, HeldObjectState) + verification_id = self._next_guard_verification_id + self._next_guard_verification_id += 1 + monitor_request = EffectVerificationRequest( + verification_id=verification_id, + skill_id=request.skill_id, + invocation_id=request.invocation_id, + invocation_revision=request.invocation_revision, + invocation_index=request.invocation_index, + attempt_generation=request.attempt_generation, + terminal_segment=request.segment_name, + requested_at=context.robot.timestamp, + deadline=request.deadline, + env_mask=observed_mask, + expected_effects=StateDelta( + held_object_updates={guard.task_state_key: candidate} + ), + ) + decision = self._observe_effect_monitor( + context, + monitor_request, + spec=guard.effect_spec, + monitor=guard.effect_monitor, + boundary_kind="in_flight_guard", + guard_id=guard.guard_id, + segment_name=request.segment_name, + ) + failure_mask |= decision.failure_mask + invalidation = self._held_object_invalidation( + guard.invalidation_task_state_keys, + failure_mask, + session.task_state, + ) + retry_mask = ( + failure_mask.clone() + if guard.retry_action + else torch.zeros_like(failure_mask) + ) + return HeldObjectGuardResult( + verification_id=request.verification_id, + object_id=self._guard_object_id(guard.effect_spec), + attempt_generation=request.attempt_generation, + invocation_index=request.invocation_index, + next_waypoint_index=request.next_waypoint_index, + failure_mask=failure_mask, + state_invalidation=invalidation, + retry_mask=retry_mask, + message=( + f"Held-object invariant {guard.guard_id!r} failed during " + f"segment {request.segment_name!r}." + if failure_mask.any() + else "" + ), + ) + + @staticmethod + def _guard_object_id(spec: SemanticEffectSpec) -> str: + """Return the canonical object ID from a single guard expectation.""" + expectation = spec.state_expectations[0] + object_id = getattr(expectation, "object_id", None) + if type(object_id) is not str or not object_id: + raise TypeError("Held-object guard expectation must own an object_id.") + return object_id + + @staticmethod + def _held_object_invalidation( + task_state_keys: tuple[str, ...], + failure_mask: torch.Tensor, + task_state: TaskState, + ) -> StateDelta: + """Build conservative removal-only reconciliation for failed rows.""" + if not failure_mask.any(): + return StateDelta() + related = set(task_state_keys) + return StateDelta( + held_object_updates={key: None for key in task_state_keys}, + coordinated_held_object_updates={ + resources: None + for resources in task_state.coordinated_held_objects + if not set(resources).isdisjoint(related) + }, + ) + + def _observe_effect_monitor( + self, + context: PlanningContext, + request: EffectVerificationRequest, + *, + spec: SemanticEffectSpec, + monitor: EffectMonitor, + boundary_kind: str = "terminal", + guard_id: str | None = None, + segment_name: str | None = None, + ) -> EffectMonitorDecision: + """Collect evidence, run one monitor, and append an auditable trace.""" + grounded = self._require_grounded() observation_revision = self._observation_revision self._observation_revision += 1 selected_env_ids = spec.env_ids[request.env_mask.to(spec.env_ids.device)] @@ -2126,13 +2339,12 @@ def _effect_verifier( configured_monitor_params=configured_monitor_params, resolved_monitor_params=resolved_monitor_params, evidence=evidence, + boundary_kind=boundary_kind, + guard_id=guard_id, + segment_name=segment_name, ) self._effect_traces.append(trace) - return EffectVerificationResult( - verification_id=request.verification_id, - success_mask=decision.success_mask, - failure_mask=decision.failure_mask, - ) + return decision def _consume_runner_step(self, runner_step: RunnerStep) -> None: """Merge one runner update into workflow-level traces.""" diff --git a/embodichain/lab/sim/solvers/base_solver.py b/embodichain/lab/sim/solvers/base_solver.py index 47d9e8dcb..a9bb9e1a5 100644 --- a/embodichain/lab/sim/solvers/base_solver.py +++ b/embodichain/lab/sim/solvers/base_solver.py @@ -13,11 +13,14 @@ # See the License for the specific language governing permissions and # limitations under the License. # ---------------------------------------------------------------------------- +from __future__ import annotations + +from abc import ABCMeta, abstractmethod +from dataclasses import fields +from typing import TYPE_CHECKING, Any, Dict, List, Tuple, Union -import torch import numpy as np -from typing import List, Dict, Any, Union, TYPE_CHECKING, Tuple -from abc import abstractmethod, ABCMeta +import torch from embodichain.utils import configclass, logger @@ -98,22 +101,39 @@ def _get_tcp_as_numpy(self) -> np.ndarray: @classmethod def from_dict(cls, init_dict: Dict[str, Any]) -> "SolverCfg": - """Initialize the configuration from a dictionary.""" + """Initialize the concrete solver configuration from a dictionary. + + The concrete config receives all recognized dataclass init fields in its + constructor so initialization and ``__post_init__`` observe the final + inputs exactly once. Legacy unannotated config attributes are applied + afterward. Unknown fields preserve the historical behavior: they are + ignored with a warning. + """ from embodichain.utils.utility import get_class_instance if "class_type" not in init_dict: logger.log_error("class type must be specified in the configuration.") - cfg = get_class_instance( + cfg_type = get_class_instance( "embodichain.lab.sim.solvers", init_dict["class_type"] + "Cfg" - )() + ) + concrete_fields = {field.name: field for field in fields(cfg_type)} + kwargs: Dict[str, Any] = {} + deferred: Dict[str, Any] = {} for key, value in init_dict.items(): - if hasattr(cfg, key): - setattr(cfg, key, value) + field = concrete_fields.get(key) + if field is not None and field.init: + kwargs[key] = value + elif field is not None or hasattr(cfg_type, key): + # A few legacy solver configs expose configurable class + # attributes without dataclass annotations. They cannot be + # constructor arguments, but remain valid serialized fields. + deferred[key] = value else: - logger.log_warning( - f"Key '{key}' not found in {cfg.__class__.__name__}." - ) + logger.log_warning(f"Key '{key}' not found in {cfg_type.__name__}.") + cfg = cfg_type(**kwargs) + for key, value in deferred.items(): + setattr(cfg, key, value) return cfg diff --git a/embodichain_tasks/configs/expert_program/tableware/hand_over.yaml b/embodichain_tasks/configs/expert_program/tableware/hand_over.yaml new file mode 100644 index 000000000..e59f18be3 --- /dev/null +++ b/embodichain_tasks/configs/expert_program/tableware/hand_over.yaml @@ -0,0 +1,41 @@ +schema_version: 1 +program_id: dual_ur5_hand_over + +integration: + robot_profile: dual_ur5_handover_v1 + scene_registry: dual_ur5_handover_v1 + runtime_preset: safe + +targets: + delivery_pose: + kind: cyclic_pose + values: + - position: [0.0, -0.2, 0.7] + quaternion_wxyz: [0.7071067812, 0.7071067812, 0.0, 0.0] + +program: + kind: segment + name: hand_over_can + steps: + kind: sequence + items: + - kind: invoke + call: + kind: pick + object: can + - kind: invoke + call: + kind: hand_over + object: can + final_target: + kind: target_ref + target: delivery_pose + post: + - kind: wait_stable + entity: can + preset: rigid_object + validators: + - kind: object_near_target + object: can + target: delivery_pose + position_tolerance: 0.12 diff --git a/embodichain_tasks/configs/gym/hand_over/dual_ur5.json b/embodichain_tasks/configs/gym/hand_over/dual_ur5.json new file mode 100644 index 000000000..7b5a45551 --- /dev/null +++ b/embodichain_tasks/configs/gym/hand_over/dual_ur5.json @@ -0,0 +1,213 @@ +{ + "id": "HandOver-v1", + "expert_program_path": "../../expert_program/tableware/hand_over.yaml", + "max_episodes": 1, + "max_episode_steps": 1200, + "num_envs": 1, + "arena_space": 3.0, + "physics_config": { + "enable_ccd": true + }, + "env": { + "sim_steps_per_control": 4, + "events": { + "settle_can_on_reset": { + "func": "wait_for_dynamic_objects_to_settle", + "mode": "reset", + "params": { + "entity_cfgs": [ + { + "uid": "handover_object" + } + ], + "min_steps": 10, + "max_steps": 120, + "check_interval_steps": 2, + "required_stable_checks": 3, + "timeout_behavior": "raise" + } + } + }, + "extensions": {} + }, + "robot": { + "uid": "DualUR5HandOver", + "urdf_cfg": { + "fname": "dual_ur5_hand_over", + "name_case": { + "joint": "lower", + "link": "lower" + }, + "components": [ + { + "component_type": "left_arm", + "urdf_path": "UniversalRobots/UR5/UR5.urdf", + "transform": [ + [0.0, -1.0, 0.0, -0.3], + [1.0, 0.0, 0.0, -1.45], + [0.0, 0.0, 1.0, 0.4], + [0.0, 0.0, 0.0, 1.0] + ] + }, + { + "component_type": "right_arm", + "urdf_path": "UniversalRobots/UR5/UR5.urdf", + "transform": [ + [0.0, -1.0, 0.0, 0.3], + [1.0, 0.0, 0.0, -1.45], + [0.0, 0.0, 1.0, 0.4], + [0.0, 0.0, 0.0, 1.0] + ] + }, + { + "component_type": "left_hand", + "urdf_path": "DH_PGI_140_80/DH_PGI_140_80.urdf" + }, + { + "component_type": "right_hand", + "urdf_path": "DH_PGI_140_80/DH_PGI_140_80.urdf" + } + ] + }, + "control_parts": { + "left_arm": ["left_joint[0-9]"], + "right_arm": ["right_joint[0-9]"], + "dual_arm": ["left_joint[0-9]", "right_joint[0-9]"], + "left_hand": ["left_gripper_finger1_joint_1"], + "right_hand": ["right_gripper_finger1_joint_1"] + }, + "drive_pros": { + "stiffness": { + "left_joint[0-9]": 10000.0, + "right_joint[0-9]": 10000.0, + "left_gripper_finger1_joint_1": 2000.0, + "right_gripper_finger1_joint_1": 2000.0, + "left_gripper_finger2_joint_1": 0.0, + "right_gripper_finger2_joint_1": 0.0 + }, + "damping": { + "left_joint[0-9]": 1000.0, + "right_joint[0-9]": 1000.0, + "left_gripper_finger1_joint_1": 50.0, + "right_gripper_finger1_joint_1": 50.0, + "left_gripper_finger2_joint_1": 0.0, + "right_gripper_finger2_joint_1": 0.0 + }, + "max_effort": { + "left_joint[0-9]": 100000.0, + "right_joint[0-9]": 100000.0, + "left_gripper_finger1_joint_1": 140.0, + "right_gripper_finger1_joint_1": 140.0, + "left_gripper_finger2_joint_1": 0.0, + "right_gripper_finger2_joint_1": 0.0 + }, + "drive_type": "force" + }, + "solver_cfg": { + "left_arm": { + "class_type": "URSolver", + "ur_type": "ur5", + "root_link_name": "left_base_link", + "end_link_name": "left_ee_link", + "tcp": [ + [1.0, 0.0, 0.0, 0.0], + [0.0, 1.0, 0.0, 0.0], + [0.0, 0.0, 1.0, 0.155], + [0.0, 0.0, 0.0, 1.0] + ], + "ik_nearest_weight": [1.0, 4.0, 1.0, 1.0, 1.0, 1.0] + }, + "right_arm": { + "class_type": "URSolver", + "ur_type": "ur5", + "root_link_name": "right_base_link", + "end_link_name": "right_ee_link", + "tcp": [ + [1.0, 0.0, 0.0, 0.0], + [0.0, 1.0, 0.0, 0.0], + [0.0, 0.0, 1.0, 0.155], + [0.0, 0.0, 0.0, 1.0] + ], + "ik_nearest_weight": [1.0, 4.0, 1.0, 1.0, 1.0, 1.0] + } + }, + "init_pos": [1.95, 0.0, 0.1], + "init_rot": [0.0, 0.0, -90.0], + "init_qpos": [ + 0.0, + 0.0, + -1.57, + -1.57, + 1.57, + 1.57, + -1.57, + -1.57, + -1.57, + -1.57, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + }, + "sensor": [], + "light": { + "direct": [ + { + "uid": "main_light", + "color": [0.6, 0.6, 0.6], + "intensity": 30.0, + "init_pos": [0.0, -0.4, 3.0] + } + ] + }, + "background": [ + { + "uid": "support_surface", + "shape": { + "shape_type": "Cube", + "size": [0.8, 1.2, 0.02] + }, + "attrs": { + "mass": 10.0, + "dynamic_friction": 0.9, + "static_friction": 0.95, + "restitution": 0.01 + }, + "body_type": "static", + "init_pos": [0.0, 0.0, 0.49], + "init_rot": [0.0, 0.0, 0.0] + } + ], + "rigid_object": [ + { + "uid": "handover_object", + "shape": { + "shape_type": "Mesh", + "fpath": "SodaCan/simple_cola_can.obj", + "compute_uv": false + }, + "attrs": { + "mass": 0.33, + "dynamic_friction": 0.97, + "static_friction": 0.99, + "angular_damping": 1.0, + "linear_damping": 0.5, + "contact_offset": 0.001, + "rest_offset": 0.0, + "restitution": 0.01, + "min_position_iters": 32, + "min_velocity_iters": 8, + "max_depenetration_velocity": 2.0 + }, + "max_convex_hull_num": 1, + "init_pos": [0.0, 0.02, 0.62], + "init_rot": [90.0, 0.0, 0.0], + "body_scale": [0.56, 0.56, 0.56] + } + ], + "rigid_object_group": [], + "articulation": [] +} diff --git a/embodichain_tasks/embodichain_tasks/tableware/__init__.py b/embodichain_tasks/embodichain_tasks/tableware/__init__.py index 4d3a1bb3c..826083302 100644 --- a/embodichain_tasks/embodichain_tasks/tableware/__init__.py +++ b/embodichain_tasks/embodichain_tasks/tableware/__init__.py @@ -18,6 +18,7 @@ from __future__ import annotations +from .hand_over import HandOverEnv from .open_drawer import OpenDrawerEnv -__all__ = ["OpenDrawerEnv"] +__all__ = ["HandOverEnv", "OpenDrawerEnv"] diff --git a/embodichain_tasks/embodichain_tasks/tableware/hand_over.py b/embodichain_tasks/embodichain_tasks/tableware/hand_over.py new file mode 100644 index 000000000..a1337f25e --- /dev/null +++ b/embodichain_tasks/embodichain_tasks/tableware/hand_over.py @@ -0,0 +1,503 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Declarative dual-UR5 can hand-over environment. + +The task owns only the physical scene, robot-resource profile, and configured +object-space hand-over poses. The packaged Expert Program selects ``pick`` and +``hand_over`` semantic calls; shared runtime components generate and execute +all arm and gripper motion. +""" + +from __future__ import annotations + +from pathlib import Path +from typing import Any + +import torch + +from embodichain.data import get_data_path +from embodichain.lab.gym.envs import EmbodiedEnv, EmbodiedEnvCfg +from embodichain.lab.gym.envs.expert_program import ( + AntipodalGraspAffordanceBinding, + ConfiguredHandOverPoseProvider, + ControlPartCommandPreset, + ControlPartEndpointBinding, + ControlPartResourceBinding, + ExpertProgramCfg, + ExpertProgramEnvironmentAdapter, + ExpertProgramEnvironmentMixin, + SimulationExpertProgramRegistration, + SimulationRigidObjectBinding, + SimulationRobotSkillProfileBinding, + SimulationSceneBinding, + create_simulation_expert_program_adapter, + load_expert_program, +) +from embodichain.lab.gym.envs.managers import EventCfg, SceneEntityCfg +from embodichain.lab.gym.envs.managers.events import ( + wait_for_dynamic_objects_to_settle, +) +from embodichain.lab.gym.utils.registration import register_env +from embodichain.lab.sim.atomic_actions import ( + BATCH_INVERSE_KINEMATICS_CAPABILITY, + CARTESIAN_POSE_CAPABILITY, + FORWARD_KINEMATICS_CAPABILITY, + GRASP_CAPABILITY, + ExecutionRunnerCfg, + HandOverOptions, + MotionPolicy, + PickUpOptions, +) +from embodichain.lab.sim.cfg import ( + LightCfg, + RigidBodyAttributesCfg, + RigidObjectCfg, + RobotCfg, +) +from embodichain.lab.sim.shapes import CubeCfg, MeshCfg +from embodichain.lab.sim.skills import SceneCollisionRole, SceneDynamics +from embodichain.lab.sim.skills.profiles import SkillPolicyPreset +from embodichain.toolkits.graspkit.pg_grasp import ( + AntipodalSamplerCfg, + GraspGeneratorCfg, + GripperCollisionCfg, +) +from embodichain_tasks.configs import get_config_path + +__all__ = [ + "HandOverEnv", + "HAND_OVER_EXPERT_PROGRAM_REGISTRATION", + "HAND_OVER_POSE_PROVIDER", + "create_hand_over_robot_profile_binding", + "create_hand_over_scene_binding", +] + +CAN_UID = "can" +CAN_SIMULATION_UID = "handover_object" +SUPPORT_SURFACE_UID = "support_surface" +HAND_OVER_SCENE_REGISTRY_ID = "dual_ur5_handover_v1" +HAND_OVER_ROBOT_PROFILE_ID = "dual_ur5_handover_v1" +HAND_OVER_GRASP_AFFORDANCE_ID = "can_antipodal_grasp" +HAND_OVER_EXPERT_PROGRAM_PATH = Path("expert_program/tableware/hand_over.yaml") + +CAN_MESH_PATH = "SodaCan/simple_cola_can.obj" +ARM_URDF_PATH = "UniversalRobots/UR5/UR5.urdf" +GRIPPER_URDF_PATH = "DH_PGI_140_80/DH_PGI_140_80.urdf" +GRIPPER_TCP_Z = 0.155 +GRIPPER_MAX_OPEN_WIDTH = 0.100 +GRIPPER_FINGER_LENGTH = 0.12 +GRIPPER_ROOT_Z_WIDTH = 0.096 +GRIPPER_Y_THICKNESS = 0.040 +GRIPPER_OPEN_QPOS = 0.0 +GRIPPER_GRASP_QPOS = 0.011 +GRIPPER_MASTER_DRIVE_STIFFNESS = 2e3 +GRIPPER_MASTER_DRIVE_DAMPING = 5e1 +GRIPPER_MASTER_DRIVE_MAX_EFFORT = 140.0 +HAND_OVER_SAMPLE_COUNT = 200 + +SUPPORT_SURFACE_Z = 0.50 +SUPPORT_SURFACE_SIZE = (0.8, 1.2, 0.02) +SUPPORT_SURFACE_CENTER = (0.0, 0.0, 0.49) +CAN_INITIAL_POSITION = (0.0, 0.02, 0.62) +CAN_INITIAL_ROTATION_DEG = (90.0, 0.0, 0.0) +CAN_SCALE = (0.56, 0.56, 0.56) +CAN_MASS = 0.33 + +_GRIPPER_TCP = ( + (1.0, 0.0, 0.0, 0.0), + (0.0, 1.0, 0.0, 0.0), + (0.0, 0.0, 1.0, GRIPPER_TCP_Z), + (0.0, 0.0, 0.0, 1.0), +) +_UR_IK_NEAREST_WEIGHT = (1.0, 4.0, 1.0, 1.0, 1.0, 1.0) +_LEFT_ARM_HOME = (0.0, 0.0, -1.57, -1.57, 1.57, 1.57) +_RIGHT_ARM_HOME = (-1.57, -1.57, -1.57, -1.57, 0.0, 0.0) +_DUAL_UR5_INIT_QPOS = (*_LEFT_ARM_HOME, *_RIGHT_ARM_HOME, 0.0, 0.0, 0.0, 0.0) + + +HAND_OVER_POSE_PROVIDER = ConfiguredHandOverPoseProvider( + middle_position=(0.0, 0.0, 0.7), + middle_quaternion_wxyz=(0.7071067812, 0.7071067812, 0.0, 0.0), + final_position=(0.0, -0.2, 0.7), + final_quaternion_wxyz=(0.7071067812, 0.7071067812, 0.0, 0.0), +) + + +def _dual_ur5_robot_dict() -> dict[str, object]: + """Return the shared serialized dual-UR5 embodiment declaration.""" + return { + "uid": "DualUR5HandOver", + "urdf_cfg": { + "fname": "dual_ur5_hand_over", + "name_case": {"joint": "lower", "link": "lower"}, + "components": [ + { + "component_type": "left_arm", + "urdf_path": ARM_URDF_PATH, + "transform": [ + [0.0, -1.0, 0.0, -0.3], + [1.0, 0.0, 0.0, -1.45], + [0.0, 0.0, 1.0, 0.4], + [0.0, 0.0, 0.0, 1.0], + ], + }, + { + "component_type": "right_arm", + "urdf_path": ARM_URDF_PATH, + "transform": [ + [0.0, -1.0, 0.0, 0.3], + [1.0, 0.0, 0.0, -1.45], + [0.0, 0.0, 1.0, 0.4], + [0.0, 0.0, 0.0, 1.0], + ], + }, + { + "component_type": "left_hand", + "urdf_path": GRIPPER_URDF_PATH, + }, + { + "component_type": "right_hand", + "urdf_path": GRIPPER_URDF_PATH, + }, + ], + }, + "control_parts": { + "left_arm": ["left_joint[0-9]"], + "right_arm": ["right_joint[0-9]"], + "dual_arm": ["left_joint[0-9]", "right_joint[0-9]"], + "left_hand": ["left_gripper_finger1_joint_1"], + "right_hand": ["right_gripper_finger1_joint_1"], + }, + "drive_pros": { + "stiffness": { + "left_joint[0-9]": 1e4, + "right_joint[0-9]": 1e4, + "left_gripper_finger1_joint_1": GRIPPER_MASTER_DRIVE_STIFFNESS, + "right_gripper_finger1_joint_1": GRIPPER_MASTER_DRIVE_STIFFNESS, + "left_gripper_finger2_joint_1": 0.0, + "right_gripper_finger2_joint_1": 0.0, + }, + "damping": { + "left_joint[0-9]": 1e3, + "right_joint[0-9]": 1e3, + "left_gripper_finger1_joint_1": GRIPPER_MASTER_DRIVE_DAMPING, + "right_gripper_finger1_joint_1": GRIPPER_MASTER_DRIVE_DAMPING, + "left_gripper_finger2_joint_1": 0.0, + "right_gripper_finger2_joint_1": 0.0, + }, + "max_effort": { + "left_joint[0-9]": 1e5, + "right_joint[0-9]": 1e5, + "left_gripper_finger1_joint_1": GRIPPER_MASTER_DRIVE_MAX_EFFORT, + "right_gripper_finger1_joint_1": GRIPPER_MASTER_DRIVE_MAX_EFFORT, + "left_gripper_finger2_joint_1": 0.0, + "right_gripper_finger2_joint_1": 0.0, + }, + "drive_type": "force", + }, + "solver_cfg": { + "left_arm": { + "class_type": "URSolver", + "ur_type": "ur5", + "root_link_name": "left_base_link", + "end_link_name": "left_ee_link", + "tcp": _GRIPPER_TCP, + "ik_nearest_weight": _UR_IK_NEAREST_WEIGHT, + }, + "right_arm": { + "class_type": "URSolver", + "ur_type": "ur5", + "root_link_name": "right_base_link", + "end_link_name": "right_ee_link", + "tcp": _GRIPPER_TCP, + "ik_nearest_weight": _UR_IK_NEAREST_WEIGHT, + }, + }, + "init_pos": [1.95, 0.0, 0.1], + "init_rot": [0.0, 0.0, -90.0], + "init_qpos": _DUAL_UR5_INIT_QPOS, + } + + +def _create_default_robot_cfg() -> RobotCfg: + """Create the dual-UR5 and dual-PGI task embodiment.""" + return RobotCfg.from_dict(_dual_ur5_robot_dict()) + + +def _load_default_expert_program() -> ExpertProgramCfg: + """Decode and preflight the packaged semantic hand-over program.""" + program = load_expert_program( + get_config_path(HAND_OVER_EXPERT_PROGRAM_PATH), + validation_context=HAND_OVER_EXPERT_PROGRAM_REGISTRATION.catalog, + ) + HAND_OVER_EXPERT_PROGRAM_REGISTRATION.catalog.preflight(program) + return program + + +def _create_default_env_cfg() -> EmbodiedEnvCfg: + """Create a directly-instantiable physical and semantic task config.""" + cfg = EmbodiedEnvCfg() + cfg.max_episode_steps = 1200 + cfg.robot = _create_default_robot_cfg() + cfg.sensor = [] + cfg.light = EmbodiedEnvCfg.EnvLightCfg( + direct=[ + LightCfg( + uid="main_light", + color=(0.6, 0.6, 0.6), + intensity=30.0, + init_pos=(0.0, -0.4, 3.0), + ) + ] + ) + cfg.background = [ + RigidObjectCfg( + uid=SUPPORT_SURFACE_UID, + shape=CubeCfg(size=list(SUPPORT_SURFACE_SIZE)), + attrs=RigidBodyAttributesCfg( + mass=10.0, + dynamic_friction=0.9, + static_friction=0.95, + restitution=0.01, + ), + body_type="static", + init_pos=list(SUPPORT_SURFACE_CENTER), + init_rot=[0.0, 0.0, 0.0], + ) + ] + cfg.rigid_object = [ + RigidObjectCfg( + uid=CAN_SIMULATION_UID, + shape=MeshCfg(fpath=get_data_path(CAN_MESH_PATH), compute_uv=False), + attrs=RigidBodyAttributesCfg( + mass=CAN_MASS, + dynamic_friction=0.97, + static_friction=0.99, + angular_damping=1.0, + linear_damping=0.5, + contact_offset=0.001, + rest_offset=0.0, + restitution=0.01, + min_position_iters=32, + min_velocity_iters=8, + max_depenetration_velocity=2.0, + ), + max_convex_hull_num=1, + init_pos=list(CAN_INITIAL_POSITION), + init_rot=list(CAN_INITIAL_ROTATION_DEG), + body_scale=CAN_SCALE, + ) + ] + cfg.extensions = {} + cfg.events = { + "settle_can_on_reset": EventCfg( + func=wait_for_dynamic_objects_to_settle, + mode="reset", + params={ + "entity_cfgs": [SceneEntityCfg(uid=CAN_SIMULATION_UID)], + "min_steps": 10, + "max_steps": 120, + "check_interval_steps": 2, + "required_stable_checks": 3, + "timeout_behavior": "raise", + }, + ) + } + cfg.expert_program = _load_default_expert_program() + return cfg + + +def create_hand_over_scene_binding( + *, + grasp_samples: int = 10000, + force_reannotate: bool = False, +) -> SimulationSceneBinding: + """Declare the can, support slab, and antipodal grasp affordance.""" + if isinstance(grasp_samples, bool) or not isinstance(grasp_samples, int): + raise TypeError("grasp_samples must be an integer.") + if grasp_samples < 1: + raise ValueError("grasp_samples must be positive.") + if not isinstance(force_reannotate, bool): + raise TypeError("force_reannotate must be a bool.") + return SimulationSceneBinding( + registry_id=HAND_OVER_SCENE_REGISTRY_ID, + rigid_objects=( + SimulationRigidObjectBinding( + entity_id=CAN_UID, + simulation_uid=CAN_SIMULATION_UID, + dynamics=SceneDynamics.DYNAMIC, + collision_role=SceneCollisionRole.NONE, + semantic_type="soda_can", + default_grasp_affordance=HAND_OVER_GRASP_AFFORDANCE_ID, + ), + SimulationRigidObjectBinding( + entity_id=SUPPORT_SURFACE_UID, + simulation_uid=SUPPORT_SURFACE_UID, + dynamics=SceneDynamics.STATIC, + collision_role=SceneCollisionRole.NONE, + semantic_type="support_surface", + ), + ), + antipodal_grasps=( + AntipodalGraspAffordanceBinding( + entity_id=HAND_OVER_GRASP_AFFORDANCE_ID, + object_id=CAN_UID, + native_name="can_mesh_antipodal", + revision="can-antipodal-v1", + generator_cfg=GraspGeneratorCfg( + viser_port=11801, + antipodal_sampler_cfg=AntipodalSamplerCfg( + n_sample=grasp_samples, + max_length=GRIPPER_MAX_OPEN_WIDTH, + min_length=0.005, + ), + is_partial_annotate=False, + is_filter_ground_collision=False, + ), + gripper_collision_cfg=GripperCollisionCfg( + max_open_length=GRIPPER_MAX_OPEN_WIDTH, + finger_length=GRIPPER_FINGER_LENGTH, + y_thickness=GRIPPER_Y_THICKNESS, + root_z_width=GRIPPER_ROOT_Z_WIDTH, + open_check_margin=0.002, + point_sample_dense=0.012, + ), + force_reannotate=force_reannotate, + ), + ), + ) + + +def create_hand_over_robot_profile_binding() -> SimulationRobotSkillProfileBinding: + """Declare left/right arm-and-gripper semantic resources.""" + motion_capabilities = frozenset( + { + BATCH_INVERSE_KINEMATICS_CAPABILITY, + CARTESIAN_POSE_CAPABILITY, + FORWARD_KINEMATICS_CAPABILITY, + } + ) + return SimulationRobotSkillProfileBinding( + profile_id=HAND_OVER_ROBOT_PROFILE_ID, + resources=tuple( + ControlPartResourceBinding( + resource_id=side, + endpoints=( + ControlPartEndpointBinding( + endpoint_id="motion", + control_part=f"{side}_arm", + capabilities=motion_capabilities, + ), + ControlPartEndpointBinding( + endpoint_id="grasp", + control_part=f"{side}_hand", + capabilities=frozenset({GRASP_CAPABILITY}), + command_preset=f"{side}_parallel_gripper", + ), + ), + ) + for side in ("left", "right") + ), + command_presets=tuple( + ControlPartCommandPreset( + preset_id=f"{side}_parallel_gripper", + control_part=f"{side}_hand", + commands={ + "open": (GRIPPER_OPEN_QPOS,), + "grasp": (GRIPPER_GRASP_QPOS,), + }, + ) + for side in ("left", "right") + ), + defaults={ + "pick_up": {"primary": "left"}, + "hand_over": {"source": "left", "destination": "right"}, + }, + presets=( + SkillPolicyPreset( + "safe", + action_option_templates={ + "pick": PickUpOptions( + pick_object_part="top", + pre_grasp_distance=0.08, + lift_height=0.10, + hand_interp_steps=5, + approach_direction=torch.tensor( + [0.0, -0.7071067812, -0.7071067812], + dtype=torch.float32, + ), + ), + "hand_over": HandOverOptions( + receive_pick_object_part="bottom", + pre_grasp_distance=0.08, + lift_height=0.08, + hand_interp_steps=10, + hold_steps=4, + retreat_steps=28, + receive_approach_direction=torch.tensor( + [0.0, 0.7071067812, -0.7071067812], + dtype=torch.float32, + ), + ), + }, + motion_policy=MotionPolicy(sample_count=HAND_OVER_SAMPLE_COUNT), + runner_cfg=ExecutionRunnerCfg( + hold_during_effect_verification=False, + hold_on_completion=False, + ), + ), + ), + default_preset="safe", + grounding_providers={ + "hand_over": ConfiguredHandOverPoseProvider.provider_id, + }, + ) + + +HAND_OVER_EXPERT_PROGRAM_REGISTRATION = SimulationExpertProgramRegistration( + scene_binding=create_hand_over_scene_binding(), + robot_profile_binding=create_hand_over_robot_profile_binding(), + handover_pose_providers=(HAND_OVER_POSE_PROVIDER,), +) + + +@register_env( + "HandOver-v1", + max_episode_steps=1200, + expert_program_registration=HAND_OVER_EXPERT_PROGRAM_REGISTRATION, +) +class HandOverEnv(ExpertProgramEnvironmentMixin, EmbodiedEnv): + """Transfer a can between two UR5 arms through a semantic program.""" + + def __init__( + self, + cfg: EmbodiedEnvCfg | None = None, + **kwargs: Any, + ) -> None: + """Initialize the configured scene without task-level motion code.""" + if cfg is None: + cfg = _create_default_env_cfg() + super().__init__(cfg, **kwargs) + self._expert_program_adapter = create_simulation_expert_program_adapter( + self, + registration=HAND_OVER_EXPERT_PROGRAM_REGISTRATION, + ) + + @property + def expert_program_adapter(self) -> ExpertProgramEnvironmentAdapter: + """Return the shared adapter assembled for this environment.""" + return self._expert_program_adapter diff --git a/tests/gym/envs/expert_program/test_simulation_environment.py b/tests/gym/envs/expert_program/test_simulation_environment.py index 5dfcb35b0..6f1a5279c 100644 --- a/tests/gym/envs/expert_program/test_simulation_environment.py +++ b/tests/gym/envs/expert_program/test_simulation_environment.py @@ -148,6 +148,7 @@ _BATCH_SIZE = 3 _ROBOT_DOF = 2 _STEP_DT = 0.04 +_UNALIGNED_PROFILE_DT = 0.01 _TRACKER_ENV_IDS = torch.tensor((7, 3, 11), dtype=torch.long) _HAND_OPEN_POSITION = 0.0 _HAND_GRASP_POSITION = 0.8 @@ -1007,8 +1008,9 @@ class _MobileRobot: def __init__(self) -> None: self.qpos = torch.zeros(_BATCH_SIZE, _ROBOT_DOF) - def get_qpos(self) -> torch.Tensor: + def get_qpos(self, *, target: bool = False) -> torch.Tensor: """Return the full controller hold state.""" + del target return self.qpos def get_qvel(self) -> torch.Tensor: @@ -1117,7 +1119,7 @@ def _profile_binding() -> SimulationRobotSkillProfileBinding: "pick": PickUpOptions(), "place": PlaceOptions(), }, - motion_policy=MotionPolicy(control_dt=0.01), + motion_policy=MotionPolicy(control_dt=_UNALIGNED_PROFILE_DT), tracking_policy=TrackingPolicy.joint_position( in_flight_max_abs_error=0.037, terminal_max_abs_error=0.019, @@ -1125,7 +1127,7 @@ def _profile_binding() -> SimulationRobotSkillProfileBinding: runner_cfg=ExecutionRunnerCfg( command_timeout=0.37, safe_stop_timeout=0.61, - minimum_cycle_time=0.04, + minimum_cycle_time=_UNALIGNED_PROFILE_DT, hold_on_completion=False, ), ), @@ -1274,17 +1276,22 @@ def _motion_generator(robot: _Robot) -> MotionGenerator: return generator -def _factory() -> tuple[SimulationExpertProgramFactory, _Robot]: +def _factory( + robot_profile_binding: SimulationRobotSkillProfileBinding | None = None, +) -> tuple[SimulationExpertProgramFactory, _Robot]: """Create one production factory around CPU-only test doubles.""" robot = _Robot() simulation = _Simulation(robot) + selected_profile_binding = ( + _profile_binding() if robot_profile_binding is None else robot_profile_binding + ) return ( SimulationExpertProgramFactory( simulation, # type: ignore[arg-type] robot, # type: ignore[arg-type] SimulationExpertProgramRegistration( scene_binding=SimulationSceneBinding(registry_id="scene"), - robot_profile_binding=_profile_binding(), + robot_profile_binding=selected_profile_binding, ), step_dt=_STEP_DT, motion_generator_factory=lambda: _motion_generator(robot), @@ -1865,17 +1872,36 @@ def _assert_invocation_equivalent( ) -def test_simulation_factory_aligns_every_motion_policy_to_gym_step() -> None: - """Cadence alignment preserves the exact registered tracking contract.""" - factory, _ = _factory() +def test_simulation_factory_aligns_every_runtime_policy_to_gym_step() -> None: + """Cadence lowering preserves source declarations and unrelated policy.""" + binding = _profile_binding() + source_preset = binding.presets[0] + source_runner_cfg = source_preset.runner_cfg + factory, _ = _factory(binding) profile = factory.create_robot_skill_profile() - assert profile.presets["safe"].motion_policy.control_dt == pytest.approx(_STEP_DT) - assert profile.presets["safe"].tracking_policy == TrackingPolicy.joint_position( + aligned_preset = profile.presets["safe"] + aligned_runner_cfg = aligned_preset.runner_cfg + assert aligned_preset.motion_policy.control_dt == pytest.approx(_STEP_DT) + assert aligned_runner_cfg.minimum_cycle_time == pytest.approx(_STEP_DT) + assert aligned_runner_cfg.command_timeout == source_runner_cfg.command_timeout + assert aligned_runner_cfg.safe_stop_timeout == source_runner_cfg.safe_stop_timeout + assert aligned_runner_cfg.hold_on_completion is source_runner_cfg.hold_on_completion + assert ( + aligned_runner_cfg.hold_during_effect_verification + is source_runner_cfg.hold_during_effect_verification + ) + assert aligned_preset.tracking_policy == TrackingPolicy.joint_position( in_flight_max_abs_error=0.037, terminal_max_abs_error=0.019, ) + assert binding.presets[0].motion_policy.control_dt == pytest.approx( + _UNALIGNED_PROFILE_DT + ) + assert binding.presets[0].runner_cfg.minimum_cycle_time == pytest.approx( + _UNALIGNED_PROFILE_DT + ) def test_mllm_config_and_atomic_skills_share_invocations_and_verified_results( diff --git a/tests/gym/envs/expert_program/test_simulation_handover.py b/tests/gym/envs/expert_program/test_simulation_handover.py new file mode 100644 index 000000000..4615b99d1 --- /dev/null +++ b/tests/gym/envs/expert_program/test_simulation_handover.py @@ -0,0 +1,85 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Tests for configured semantic hand-over pose integration.""" + +from __future__ import annotations + +import pytest +import torch + +from embodichain.lab.gym.envs.expert_program import ConfiguredHandOverPoseProvider + + +def _provider() -> ConfiguredHandOverPoseProvider: + """Return one deterministic dual-arm transfer declaration.""" + return ConfiguredHandOverPoseProvider( + middle_position=(0.0, 0.0, 0.7), + middle_quaternion_wxyz=(1.0, 1.0, 0.0, 0.0), + final_position=(0.0, -0.2, 0.7), + final_quaternion_wxyz=(1.0, 1.0, 0.0, 0.0), + ) + + +def test_configured_handover_provider_normalizes_and_owns_targets() -> None: + """Configured poses are normalized and returned as independent values.""" + provider = _provider() + + first = provider.resolve(object(), context=object(), bound=object()) + second = provider.resolve(object(), context=object(), bound=object()) + + expected_rotation = torch.tensor( + [ + [1.0, 0.0, 0.0], + [0.0, 0.0, -1.0], + [0.0, 1.0, 0.0], + ] + ) + assert first.middle.pose is not second.middle.pose + assert first.final.pose is not second.final.pose + assert torch.allclose( + first.middle.pose.to_matrix()[:3, :3], expected_rotation, atol=1e-6 + ) + assert torch.allclose( + first.middle.pose.to_matrix()[:3, 3], torch.tensor([0.0, 0.0, 0.7]) + ) + assert torch.allclose( + first.final.pose.to_matrix()[:3, 3], torch.tensor([0.0, -0.2, 0.7]) + ) + + +@pytest.mark.parametrize( + ("overrides", "error_type"), + [ + ({"middle_position": (0.0, 0.0)}, TypeError), + ({"middle_quaternion_wxyz": (0.0, 0.0, 0.0, 0.0)}, ValueError), + ], +) +def test_configured_handover_provider_rejects_invalid_declarations( + overrides: dict[str, object], + error_type: type[Exception], +) -> None: + """Malformed provider declarations fail before simulation construction.""" + values: dict[str, object] = { + "middle_position": (0.0, 0.0, 0.7), + "middle_quaternion_wxyz": (1.0, 0.0, 0.0, 0.0), + "final_position": (0.0, -0.2, 0.7), + "final_quaternion_wxyz": (1.0, 0.0, 0.0, 0.0), + } + values.update(overrides) + + with pytest.raises(error_type): + ConfiguredHandOverPoseProvider(**values) # type: ignore[arg-type] diff --git a/tests/gym/envs/expert_program/test_simulation_policies.py b/tests/gym/envs/expert_program/test_simulation_policies.py index 51b3bea13..5ad6a2e1c 100644 --- a/tests/gym/envs/expert_program/test_simulation_policies.py +++ b/tests/gym/envs/expert_program/test_simulation_policies.py @@ -85,15 +85,22 @@ def get_local_pose(self, *, to_matrix: bool) -> torch.Tensor: class _Robot: - """Full-qpos source used by post-policy hold actions.""" + """Distinct current- and target-qpos source for post-policy holds.""" - def __init__(self, qpos: torch.Tensor) -> None: - self.qpos = qpos - self.qpos_reads = 0 + def __init__( + self, + current_qpos: torch.Tensor, + target_qpos: torch.Tensor | None = None, + ) -> None: + self.current_qpos = current_qpos.clone() + self.target_qpos = ( + current_qpos.clone() if target_qpos is None else target_qpos.clone() + ) + self.qpos_reads: list[bool] = [] - def get_qpos(self) -> torch.Tensor: - self.qpos_reads += 1 - return self.qpos.clone() + def get_qpos(self, target: bool = False) -> torch.Tensor: + self.qpos_reads.append(target) + return (self.target_qpos if target else self.current_qpos).clone() class _Simulation: @@ -177,10 +184,14 @@ def _port( positions: torch.Tensor, *, preset: DynamicSettleMonitorCfg | None = None, + target_qpos: torch.Tensor | None = None, ) -> tuple[SimulationSegmentPolicyPort, _RigidObject, _Robot]: """Build one policy port and expose its mutable test doubles.""" entity = _RigidObject(positions) - robot = _Robot(torch.tensor([[1.0, 2.0], [3.0, 4.0]])) + robot = _Robot( + torch.tensor([[1.0, 2.0], [3.0, 4.0]]), + target_qpos=target_qpos, + ) port = SimulationSegmentPolicyPort( _Simulation(entity), robot, @@ -226,7 +237,7 @@ def test_pure_preflight_validates_hooks_without_reading_live_state() -> None: port.validate_policy(segment.post_policies[0], segment=segment) port.validate_validator(segment.validators[0], segment=segment) - assert robot.qpos_reads == 1 + assert robot.qpos_reads == [False] assert entity.pose_reads == 0 @@ -238,14 +249,24 @@ def test_pure_preflight_rejects_unknown_settle_preset_without_observation() -> N with pytest.raises(KeyError, match="Unknown settle preset 'missing'"): port.validate_policy(segment.post_policies[0], segment=segment) - assert robot.qpos_reads == 1 + assert robot.qpos_reads == [False] assert entity.pose_reads == 0 -def test_wait_stable_yields_fresh_full_qpos_holds_through_gym() -> None: - """Settling observes only after each yielded hold has been consumed.""" +def test_wait_stable_yields_fresh_target_qpos_holds_through_gym() -> None: + """Settling preserves loaded drive targets with independently owned holds.""" segment = _compiled_segment() - port, _, robot = _port(torch.zeros(2, 3)) + target_qpos = torch.tensor([[5.0, 6.0], [7.0, 8.0]]) + port, _, robot = _port( + torch.zeros(2, 3), + target_qpos=target_qpos, + preset=DynamicSettleMonitorCfg( + min_steps=0, + max_steps=4, + check_interval_steps=1, + required_stable_checks=3, + ), + ) actions = port.actions( segment.post_policies[0], segment=segment, @@ -253,11 +274,21 @@ def test_wait_stable_yields_fresh_full_qpos_holds_through_gym() -> None: ) first = next(actions) - assert torch.equal(first, robot.qpos) + assert torch.equal(first, target_qpos) + assert not torch.equal(first, robot.current_qpos) first.fill_(99.0) + + second = next(actions) + assert torch.equal(second, target_qpos) + assert second.data_ptr() != first.data_ptr() with pytest.raises(StopIteration): next(actions) - assert torch.equal(robot.qpos, torch.tensor([[1.0, 2.0], [3.0, 4.0]])) + assert torch.equal( + robot.current_qpos, + torch.tensor([[1.0, 2.0], [3.0, 4.0]]), + ) + assert torch.equal(robot.target_qpos, target_qpos) + assert robot.qpos_reads == [False, True, True] metadata = port.post_policy_metadata( segment.post_policies[0], @@ -269,11 +300,11 @@ def test_wait_stable_yields_fresh_full_qpos_holds_through_gym() -> None: "linear_velocity": 0.03, "angular_velocity": 0.2, "min_steps": 0, - "max_steps": 3, + "max_steps": 4, "check_interval_steps": 1, - "required_stable_checks": 2, + "required_stable_checks": 3, } - assert metadata["state"]["elapsed_steps"] == 1 + assert metadata["state"]["elapsed_steps"] == 2 assert metadata["state"]["settled_mask"] == [True, True] assert metadata["state"]["timeout_mask"] == [False, False] assert metadata["state"]["max_linear_speed"] == [0.0, 0.0] @@ -283,6 +314,66 @@ def test_wait_stable_yields_fresh_full_qpos_holds_through_gym() -> None: ).tolist() == [True, True] +def test_wait_stable_holds_active_targets_and_inactive_current_qpos() -> None: + """Initial inactive rows use measured holds while active rows keep preload.""" + segment = _compiled_segment() + target_qpos = torch.tensor([[5.0, 6.0], [7.0, 8.0]]) + port, _, robot = _port( + torch.zeros(2, 3), + target_qpos=target_qpos, + preset=DynamicSettleMonitorCfg( + min_steps=0, + max_steps=4, + check_interval_steps=1, + required_stable_checks=3, + ), + ) + active_mask = torch.tensor([True, False]) + actions = port.actions( + segment.post_policies[0], + segment=segment, + active_mask=active_mask, + ) + expected = torch.stack((target_qpos[0], robot.current_qpos[1])) + + first = next(actions) + assert torch.equal(first, expected) + first.fill_(99.0) + + second = next(actions) + assert torch.equal(second, expected) + assert second.data_ptr() != first.data_ptr() + with pytest.raises(StopIteration): + next(actions) + + assert robot.qpos_reads == [False, True, False, True, False] + result = port.post_policy_result( + segment.post_policies[0], + segment=segment, + ) + assert result.tolist() == [True, False] + + +def test_wait_stable_rejects_wrong_target_width_for_all_active_rows() -> None: + """All-active settling fails closed on a malformed full target qpos.""" + segment = _compiled_segment() + port, _, robot = _port(torch.zeros(2, 3)) + robot.target_qpos = torch.zeros(2, 3) + actions = port.actions( + segment.post_policies[0], + segment=segment, + active_mask=torch.ones(2, dtype=torch.bool), + ) + + with pytest.raises( + ValueError, + match="target full qpos must match the construction-time current full qpos", + ): + next(actions) + + assert robot.qpos_reads == [False, True] + + def test_wait_stable_returns_row_local_timeout_result_and_metadata() -> None: """A moving row times out without failing a settled peer or the batch.""" segment = _compiled_segment() diff --git a/tests/gym/envs/tasks/test_hand_over.py b/tests/gym/envs/tasks/test_hand_over.py new file mode 100644 index 000000000..7f7293e42 --- /dev/null +++ b/tests/gym/envs/tasks/test_hand_over.py @@ -0,0 +1,541 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Tests for the declarative dual-UR5 hand-over task.""" + +from __future__ import annotations + +import importlib +import json +from pathlib import Path +from types import SimpleNamespace + +import pytest +import torch + +from embodichain.lab.gym.envs import EmbodiedEnv +from embodichain.lab.gym.envs.demo import execute_demo_episode +from embodichain.lab.gym.envs.expert_program import ( + ConfiguredHandOverPoseProvider, + ExpertProgramEnvironmentMixin, +) +from embodichain.lab.gym.utils.gym_utils import config_to_cfg +from embodichain.lab.gym.utils.registration import ( + REGISTERED_ENVS, + discover_task_packages, +) +from embodichain.lab.sim.atomic_actions import HandOverOptions, PickUpOptions +from embodichain.lab.sim.cfg import RobotCfg +from embodichain.lab.sim.skills import HandOver, Pick + +# Trigger official task auto-registration (idempotent). +discover_task_packages() + +from embodichain_tasks.tableware.hand_over import ( # noqa: E402 + CAN_SIMULATION_UID, + CAN_UID, + CAN_MASS, + GRIPPER_MASTER_DRIVE_DAMPING, + GRIPPER_MASTER_DRIVE_MAX_EFFORT, + GRIPPER_MASTER_DRIVE_STIFFNESS, + GRIPPER_GRASP_QPOS, + GRIPPER_OPEN_QPOS, + HAND_OVER_EXPERT_PROGRAM_REGISTRATION, + HAND_OVER_POSE_PROVIDER, + HAND_OVER_ROBOT_PROFILE_ID, + HAND_OVER_SCENE_REGISTRY_ID, + HAND_OVER_SAMPLE_COUNT, + SUPPORT_SURFACE_UID, + HandOverEnv, + _create_default_env_cfg, + create_hand_over_robot_profile_binding, + create_hand_over_scene_binding, +) + +EXPECTED_GRIPPER_GRASP_QPOS = 0.011 + + +def _gym_config_path() -> Path: + """Return the installed-source dual-UR5 Gym config path.""" + return ( + Path(__file__).parents[4] + / "embodichain_tasks/configs/gym/hand_over/dual_ur5.json" + ) + + +def _gym_payload() -> dict[str, object]: + """Load the runnable HandOver Gym config as inert JSON data.""" + payload = json.loads(_gym_config_path().read_text(encoding="utf-8")) + assert type(payload) is dict + return payload + + +def test_registered_hand_over_task_uses_shared_expert_program_mixin() -> None: + """The task registers one semantic environment without local demo code.""" + from embodichain_tasks.tableware import __all__ + + assert "HandOverEnv" in __all__ + spec = REGISTERED_ENVS["HandOver-v1"] + assert spec.cls is HandOverEnv + assert spec.max_episode_steps == 1200 + assert spec.expert_program_registration is HAND_OVER_EXPERT_PROGRAM_REGISTRATION + assert "expert_program_registration" not in spec.default_kwargs + assert issubclass(HandOverEnv, ExpertProgramEnvironmentMixin) + assert issubclass(HandOverEnv, EmbodiedEnv) + assert "create_demo_action_list" not in HandOverEnv.__dict__ + + +def test_hand_over_gym_config_selects_packaged_program_without_contact_sensor() -> None: + """Normal startup selects the semantic program and needs no contact sensor.""" + payload = _gym_payload() + + assert payload["id"] == "HandOver-v1" + assert payload["expert_program_path"] == ( + "../../expert_program/tableware/hand_over.yaml" + ) + assert payload["sensor"] == [] + assert payload["env"]["extensions"] == {} + settle = payload["env"]["events"]["settle_can_on_reset"] + assert settle["func"] == "wait_for_dynamic_objects_to_settle" + assert settle["params"]["entity_cfgs"] == [{"uid": CAN_SIMULATION_UID}] + + +def test_hand_over_gym_config_builds_dual_ur5_pgi_scene() -> None: + """Config parsing preserves the tutorial robot, can, and support geometry.""" + path = _gym_config_path() + cfg = config_to_cfg(_gym_payload(), source_path=path) + + assert type(cfg.robot) is RobotCfg + assert cfg.robot.uid == "DualUR5HandOver" + assert cfg.robot.control_parts["left_arm"] == ["left_joint[0-9]"] + assert cfg.robot.control_parts["right_arm"] == ["right_joint[0-9]"] + assert cfg.robot.control_parts["left_hand"] == ["left_gripper_finger1_joint_1"] + assert cfg.robot.control_parts["right_hand"] == ["right_gripper_finger1_joint_1"] + assert set(cfg.robot.urdf_cfg.components) == { + "left_arm", + "right_arm", + "left_hand", + "right_hand", + } + assert cfg.robot.solver_cfg["left_arm"].ik_nearest_weight == [ + 1.0, + 4.0, + 1.0, + 1.0, + 1.0, + 1.0, + ] + assert cfg.robot.solver_cfg["left_arm"].root_link_name == "left_base_link" + assert cfg.robot.solver_cfg["left_arm"].end_link_name == "left_ee_link" + assert cfg.robot.solver_cfg["right_arm"].root_link_name == "right_base_link" + assert cfg.robot.solver_cfg["right_arm"].end_link_name == "right_ee_link" + assert cfg.robot.solver_cfg["right_arm"].tcp[2][3] == pytest.approx(0.155) + assert list(cfg.robot.init_qpos) == pytest.approx( + [ + 0.0, + 0.0, + -1.57, + -1.57, + 1.57, + 1.57, + -1.57, + -1.57, + -1.57, + -1.57, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + ] + ) + assert [item.uid for item in cfg.background] == [SUPPORT_SURFACE_UID] + assert [item.uid for item in cfg.rigid_object] == [CAN_SIMULATION_UID] + assert cfg.rigid_object[0].max_convex_hull_num == 1 + assert cfg.expert_program is not None + assert cfg.expert_program.program_id == "dual_ur5_hand_over" + + +def test_hand_over_physics_configs_match_tuned_can_and_pgi_parameters() -> None: + """Python and JSON configs share real can and master-only PGI dynamics.""" + direct_cfg = _create_default_env_cfg() + json_cfg = config_to_cfg(_gym_payload(), source_path=_gym_config_path()) + + expected_values = { + "stiffness": GRIPPER_MASTER_DRIVE_STIFFNESS, + "damping": GRIPPER_MASTER_DRIVE_DAMPING, + "max_effort": GRIPPER_MASTER_DRIVE_MAX_EFFORT, + } + for cfg in (direct_cfg, json_cfg): + assert cfg.rigid_object[0].attrs.mass == pytest.approx(CAN_MASS) + drive = cfg.robot.drive_pros + for property_name, master_value in expected_values.items(): + values = getattr(drive, property_name) + for side in ("left", "right"): + assert values[f"{side}_gripper_finger1_joint_1"] == pytest.approx( + master_value + ) + assert values[f"{side}_gripper_finger2_joint_1"] == pytest.approx(0.0) + + +def test_hand_over_registration_owns_scene_and_pose_provider() -> None: + """Static registration fingerprints the exact grasp and pose declarations.""" + scene = create_hand_over_scene_binding() + grasp = scene.antipodal_grasps[0] + + assert scene.registry_id == HAND_OVER_SCENE_REGISTRY_ID + assert [item.entity_id for item in scene.rigid_objects] == [ + CAN_UID, + SUPPORT_SURFACE_UID, + ] + assert scene.rigid_objects[0].simulation_uid == CAN_SIMULATION_UID + assert grasp.object_id == CAN_UID + assert grasp.generator_cfg.antipodal_sampler_cfg.n_sample == 10000 + assert grasp.force_reannotate is False + assert HAND_OVER_EXPERT_PROGRAM_REGISTRATION.scene_binding == scene + assert HAND_OVER_EXPERT_PROGRAM_REGISTRATION.handover_pose_providers == ( + HAND_OVER_POSE_PROVIDER, + ) + assert ( + ConfiguredHandOverPoseProvider.provider_id + == "simulation.configured_handover_pose" + ) + assert HAND_OVER_POSE_PROVIDER.middle_position == pytest.approx((0.0, 0.0, 0.7)) + assert HAND_OVER_POSE_PROVIDER.final_position == pytest.approx((0.0, -0.2, 0.7)) + + +def test_hand_over_profile_binds_left_pick_and_left_to_right_transfer() -> None: + """The profile selects both participants and its tuned motion policy.""" + binding = create_hand_over_robot_profile_binding() + + assert binding.profile_id == HAND_OVER_ROBOT_PROFILE_ID + assert [resource.resource_id for resource in binding.resources] == [ + "left", + "right", + ] + assert [ + endpoint.control_part + for resource in binding.resources + for endpoint in resource.endpoints + ] == ["left_arm", "left_hand", "right_arm", "right_hand"] + assert dict(binding.defaults["pick_up"]) == {"primary": "left"} + assert dict(binding.defaults["hand_over"]) == { + "source": "left", + "destination": "right", + } + assert binding.presets[0].preset_id == "safe" + assert binding.presets[0].motion_policy.sample_count == HAND_OVER_SAMPLE_COUNT + assert binding.presets[0].runner_cfg.hold_during_effect_verification is False + assert binding.presets[0].runner_cfg.hold_on_completion is False + templates = binding.presets[0].action_option_templates + pick_options = templates["pick"] + assert type(pick_options) is PickUpOptions + assert pick_options.pick_object_part == "top" + assert pick_options.pre_grasp_distance == pytest.approx(0.08) + assert pick_options.lift_height == pytest.approx(0.10) + assert pick_options.hand_interp_steps == 5 + torch.testing.assert_close( + pick_options.approach_direction, + torch.tensor([0.0, -0.7071067812, -0.7071067812]), + ) + hand_over_options = templates["hand_over"] + assert type(hand_over_options) is HandOverOptions + assert hand_over_options.receive_pick_object_part == "bottom" + assert hand_over_options.pre_grasp_distance == pytest.approx(0.08) + assert hand_over_options.lift_height == pytest.approx(0.08) + assert hand_over_options.hand_interp_steps == 10 + assert hand_over_options.hold_steps == 4 + assert hand_over_options.retreat_steps == 28 + torch.testing.assert_close( + hand_over_options.receive_approach_direction, + torch.tensor([0.0, 0.7071067812, -0.7071067812]), + ) + assert dict(binding.grounding_providers) == { + "hand_over": ConfiguredHandOverPoseProvider.provider_id, + } + for side, preset in zip(("left", "right"), binding.command_presets): + assert preset.control_part == f"{side}_hand" + assert tuple(preset.commands["open"]) == (GRIPPER_OPEN_QPOS,) + assert tuple(preset.commands["grasp"]) == (GRIPPER_GRASP_QPOS,) + assert tuple(preset.commands["grasp"]) == pytest.approx( + (EXPECTED_GRIPPER_GRASP_QPOS,) + ) + + +def test_direct_default_cfg_loads_the_registered_semantic_program() -> None: + """Direct construction and JSON startup select the same registration IDs.""" + cfg = _create_default_env_cfg() + + assert type(cfg.robot) is RobotCfg + assert cfg.sensor == [] + assert cfg.expert_program is not None + assert cfg.expert_program.integration.scene_registry == HAND_OVER_SCENE_REGISTRY_ID + assert cfg.expert_program.integration.robot_profile == HAND_OVER_ROBOT_PROFILE_ID + assert cfg.expert_program.integration.runtime_preset == "safe" + settle = cfg.events["settle_can_on_reset"] + assert settle.params["entity_cfgs"][0].uid == CAN_SIMULATION_UID + + +def test_task_initialization_passes_only_registration_to_shared_factory( + monkeypatch, +) -> None: + """Task setup has no provider side channel or local motion generator.""" + adapter = object() + captured: dict[str, object] = {} + + def fake_base_init(self, cfg, **kwargs) -> None: + del self, cfg, kwargs + + def fake_create_adapter(environment, **kwargs): + captured["environment"] = environment + captured.update(kwargs) + return adapter + + monkeypatch.setattr(EmbodiedEnv, "__init__", fake_base_init) + task_module = importlib.import_module(HandOverEnv.__module__) + monkeypatch.setattr( + task_module, + "create_simulation_expert_program_adapter", + fake_create_adapter, + ) + + env = HandOverEnv(cfg=object()) + + assert env.expert_program_adapter is adapter + assert captured == { + "environment": env, + "registration": HAND_OVER_EXPERT_PROGRAM_REGISTRATION, + } + + +def test_task_config_compiles_through_real_simulation_factory(monkeypatch) -> None: + """The packaged program reaches the real adapter through explicit mocks.""" + + class FakeRobot: + uid = "DualUR5HandOver" + + @staticmethod + def get_qpos(*, target: bool = False) -> torch.Tensor: + del target + return torch.zeros((1, 16), dtype=torch.float32) + + class FakeRigidObject: + def __init__(self, *, is_non_dynamic: bool) -> None: + self.is_non_dynamic = is_non_dynamic + + robot = FakeRobot() + can = FakeRigidObject(is_non_dynamic=False) + support = FakeRigidObject(is_non_dynamic=True) + + class FakeSimulation: + @staticmethod + def get_robot(uid: str): + return robot if uid == robot.uid else None + + @staticmethod + def get_rigid_object(uid: str): + return { + CAN_SIMULATION_UID: can, + SUPPORT_SURFACE_UID: support, + }.get(uid) + + def fake_base_init(self, cfg, **kwargs) -> None: + del kwargs + self.cfg = cfg + self.sim_cfg = SimpleNamespace(physics_dt=0.01) + self.sim = FakeSimulation() + self.robot = robot + + monkeypatch.setattr(EmbodiedEnv, "__init__", fake_base_init) + cfg = config_to_cfg(_gym_payload(), source_path=_gym_config_path()) + + env = HandOverEnv(cfg=cfg) + segments = tuple(env.compile_expert_program(cfg.expert_program)) + + assert len(segments) == 1 + assert segments[0].name == "hand_over_can" + assert [type(call.call) for call in segments[0].calls] == [Pick, HandOver] + assert len(segments[0].post_policies) == 1 + assert len(segments[0].validators) == 1 + assert env.expert_program_adapter.scene_registry_id == (HAND_OVER_SCENE_REGISTRY_ID) + assert env.expert_program_adapter.robot_profile_id == (HAND_OVER_ROBOT_PROFILE_ID) + + +@pytest.mark.requires_sim +@pytest.mark.slow +def test_real_sim_expert_episode_transfers_can_with_effect_and_validation_trace() -> ( + None +): + """The full semantic episode proves transfer effects, settling, and validation.""" + import gc + + from embodichain.lab.sim import SimulationManager, SimulationManagerCfg + + cfg = config_to_cfg(_gym_payload(), source_path=_gym_config_path()) + cfg.num_envs = 1 + cfg.sim_cfg = SimulationManagerCfg( + headless=True, + sim_device="cpu", + num_envs=1, + ) + cfg.sensor = [] + cfg.observations = None + cfg.dataset = None + cfg.init_rollout_buffer = False + cfg.record_trajectory = False + cfg.filter_dataset_saving = True + + env: HandOverEnv | None = None + try: + env = HandOverEnv(cfg=cfg) + env.reset(seed=0) + can = env.sim.get_rigid_object(CAN_SIMULATION_UID) + assert can is not None + initial_can_pose = can.get_local_pose(to_matrix=True).tolist() + initial_left_eef = env.robot.compute_fk( + env.robot.get_qpos(name="left_arm"), + name="left_arm", + to_matrix=True, + ).tolist() + initial_qpos = env.robot.get_qpos().tolist() + + result = execute_demo_episode(env) + + if not result.completed: + runtime = result.segments[0].metadata["runtime"] + failed_call = runtime["calls"][-1] + last_effect = ( + None if not failed_call["effects"] else failed_call["effects"][-1] + ) + pytest.fail( + json.dumps( + { + "terminal_reason": result.terminal_reason, + "initial_can_pose": initial_can_pose, + "initial_left_eef": initial_left_eef, + "initial_qpos": initial_qpos, + "final_can_pose": can.get_local_pose(to_matrix=True).tolist(), + "final_left_eef": env.robot.compute_fk( + env.robot.get_qpos(name="left_arm"), + name="left_arm", + to_matrix=True, + ).tolist(), + "final_left_hand_qpos": env.robot.get_qpos( + name="left_hand" + ).tolist(), + "events": [ + { + "kind": event["kind"], + "timestamp": event["timestamp"], + "message": event["message"], + } + for event in runtime["events"] + ], + "plan_success_masks": [ + attempt["plan_success_mask"] + for attempt in failed_call["plan_attempts"] + ], + "last_effect": last_effect, + "post_policies": result.segments[0].metadata["post_policies"], + "validation": result.segments[0].metadata["validation"], + }, + sort_keys=True, + ), + pytrace=False, + ) + assert result.all_success + assert result.terminal_reason == "success" + assert len(result.segments) == 1 + segment = result.segments[0] + assert segment.name == "hand_over_can" + assert segment.success + + metadata = segment.metadata + runtime = metadata["runtime"] + assert runtime["kind"] == "skill_result" + assert runtime["status"] == "completed" + assert runtime["masks"]["success"] == [True] + assert [call["semantic_id"] for call in runtime["calls"]] == [ + "pick", + "hand_over", + ] + for call in runtime["calls"]: + assert call["status"] == "completed" + assert call["masks"] == { + "entered": [True], + "completed": [True], + "failed": [False], + } + assert call["plan_attempts"] + assert call["plan_attempts"][-1]["plan_success_mask"] == [True] + assert call["effects"] + assert call["effects"][-1]["decision"] == { + "success_mask": [True], + "failure_mask": [False], + } + + pick_effect = runtime["calls"][0]["effects"][-1] + assert pick_effect["effect_spec"]["semantic_id"] == "pick" + assert set(pick_effect["evidence"]) == { + "destination.pose", + "destination.constraint", + } + assert pick_effect["evidence"]["destination.constraint"]["values"] == [True] + + transfer_effect = runtime["calls"][1]["effects"][-1] + assert transfer_effect["effect_spec"]["semantic_id"] == "hand_over" + assert set(transfer_effect["evidence"]) == { + "source.pose", + "source.constraint", + "destination.pose", + "destination.constraint", + } + for evidence in transfer_effect["evidence"].values(): + assert evidence["valid_mask"] == [True] + assert evidence["acquisition_errors"] == [None] + assert evidence["env_ids"] == [0] + assert transfer_effect["evidence"]["source.constraint"]["values"] == [False] + assert transfer_effect["evidence"]["destination.constraint"]["values"] == [True] + + post_policies = metadata["post_policies"] + assert len(post_policies) == 1 + assert post_policies[0]["kind"] == "wait_stable" + assert post_policies[0]["result_mask"] == [True] + assert post_policies[0]["result"]["status"] == "settled" + assert post_policies[0]["result"]["state"]["settled_mask"] == [True] + assert post_policies[0]["result"]["state"]["timeout_mask"] == [False] + + validation = metadata["validation"] + assert validation["runtime_success_mask"] == [True] + assert validation["eligible_mask_before_validation"] == [True] + assert validation["post_policy_success_mask"] == [True] + assert validation["accepted_mask"] == [True] + assert len(validation["validators"]) == 1 + validator = validation["validators"][0] + assert validator["kind"] == "object_near_target" + assert validator["result_mask"] == [True] + assert validator["result"]["accepted_mask"] == [True] + assert validator["result"]["position_tolerance"] == pytest.approx(0.12) + assert validator["result"]["position_error"][0] <= 0.12 + finally: + if env is not None: + env.close() + SimulationManager.flush_cleanup_queue() + gc.collect() + + +__all__: list[str] = [] diff --git a/tests/sim/atomic_actions/test_engine_per_env.py b/tests/sim/atomic_actions/test_engine_per_env.py index b81c96e33..85bf987f5 100644 --- a/tests/sim/atomic_actions/test_engine_per_env.py +++ b/tests/sim/atomic_actions/test_engine_per_env.py @@ -49,6 +49,8 @@ EffectVerificationRequirement, EffectVerificationResult, GraspGoal, + HeldObjectGuardRequest, + HeldObjectGuardResult, HeldObjectState, JointPositionPayload, JointPositionTarget, @@ -508,6 +510,54 @@ def _context( ) +def _with_held_object( + context: PlanningContext, + *, + env_mask: torch.Tensor | None = None, +) -> PlanningContext: + """Attach one verified test object to the logical arm resource.""" + semantics = ObjectSemantics( + affordance=Affordance(), + geometry={}, + label="object", + entity_id="object", + ) + held = HeldObjectState( + semantics=semantics, + object_to_eef=torch.eye(4), + grasp_xpos=torch.eye(4), + env_mask=env_mask, + ) + return replace( + context, + task=TaskState( + batch_size=context.batch_size, + device=context.robot.qpos.device, + held_objects={"arm": held}, + ), + ) + + +def _held_object_loss_result( + request: HeldObjectGuardRequest, + *, + failure_mask: torch.Tensor, + retry_mask: torch.Tensor, +) -> HeldObjectGuardResult: + """Build a loss result exactly correlated with one guard request.""" + return HeldObjectGuardResult( + verification_id=request.verification_id, + object_id="object", + attempt_generation=request.attempt_generation, + invocation_index=request.invocation_index, + next_waypoint_index=request.next_waypoint_index, + failure_mask=failure_mask, + state_invalidation=StateDelta(held_object_updates={"arm": None}), + retry_mask=retry_mask, + message="Observed object-to-endpoint slip.", + ) + + def _multi_dependency_context( timestamp: float, *, @@ -697,6 +747,203 @@ def test_session_completes_incremental_command_sequence() -> None: assert final.eligible_mask.tolist() == [True] +def test_held_object_loss_retries_only_failed_row_with_reconciled_state() -> None: + engine, _ = _engine(batch_size=2) + initial = _with_held_object(_context(0.0, (0.0, 0.0), (0.2, 0.2), 0)) + session = engine.start( + (_invocation(engine, max_action_retries=1),), + initial, + ) + request = session.held_object_guard_request + assert request is not None + assert request.attempt_generation == 0 + assert request.invocation_index == 0 + assert request.next_waypoint_index == 0 + assert request.segment_name == "dynamic" + assert request.env_mask.tolist() == [True, True] + assert request.allowed_held_object_relations == (("arm", "object"),) + assert request.allowed_coordinated_held_object_relations == () + + retried = session.tick( + initial, + held_object_guard_result=_held_object_loss_result( + request, + failure_mask=torch.tensor([True, False]), + retry_mask=torch.tensor([True, False]), + ), + ) + + assert retried.status is ExecutionStatus.RUNNING + assert retried.command is not None + assert retried.command.active_mask.tolist() == [True, True] + held = retried.task_state.get_held_object("arm") + assert held is not None and held.env_mask is not None + assert held.env_mask.tolist() == [False, True] + lost = next( + event + for event in retried.events + if event.kind is ExecutionEventKind.HELD_OBJECT_LOST + ) + retry = next( + event + for event in retried.events + if event.kind is ExecutionEventKind.ACTION_RETRY + ) + assert lost.env_mask.tolist() == [True, False] + assert retry.env_mask.tolist() == [True, False] + assert session.plan_attempts[-1].action_retry_counts == (1, 0) + + +def test_held_object_loss_result_requires_state_invalidation() -> None: + with pytest.raises(ValueError, match="must contain relation removals"): + HeldObjectGuardResult( + verification_id=0, + object_id="object", + attempt_generation=0, + invocation_index=0, + next_waypoint_index=0, + failure_mask=torch.tensor([True]), + state_invalidation=StateDelta(), + retry_mask=torch.tensor([False]), + ) + + +def test_held_object_guard_rejects_unauthorized_state_invalidation() -> None: + engine, _ = _engine() + initial = _with_held_object(_context(0.0, 0.0, 0.2, 0)) + session = engine.start((_invocation(engine),), initial) + request = session.held_object_guard_request + assert request is not None + + result = HeldObjectGuardResult( + verification_id=request.verification_id, + object_id="object", + attempt_generation=request.attempt_generation, + invocation_index=request.invocation_index, + next_waypoint_index=request.next_waypoint_index, + failure_mask=torch.tensor([True]), + state_invalidation=StateDelta(held_object_updates={"unrelated_resource": None}), + retry_mask=torch.tensor([False]), + ) + with pytest.raises(ValueError, match="authorized relation set"): + session.tick(initial, held_object_guard_result=result) + + +def test_held_object_guard_rejects_wrong_object_identity_on_authorized_key() -> None: + engine, _ = _engine() + initial = _with_held_object(_context(0.0, 0.0, 0.2, 0)) + session = engine.start((_invocation(engine),), initial) + request = session.held_object_guard_request + assert request is not None + + result = HeldObjectGuardResult( + verification_id=request.verification_id, + object_id="another_object", + attempt_generation=request.attempt_generation, + invocation_index=request.invocation_index, + next_waypoint_index=request.next_waypoint_index, + failure_mask=torch.tensor([True]), + state_invalidation=StateDelta(held_object_updates={"arm": None}), + retry_mask=torch.tensor([False]), + ) + with pytest.raises(ValueError, match="key/object identity"): + session.tick(initial, held_object_guard_result=result) + + +def test_stale_held_object_guard_result_is_rejected_within_same_attempt() -> None: + engine, _ = _engine() + initial = _with_held_object(_context(0.0, 0.0, 0.2, 0)) + session = engine.start((_invocation(engine),), initial) + first_request = session.held_object_guard_request + assert first_request is not None + + session.tick(initial) + current_request = session.held_object_guard_request + assert current_request is not None + assert current_request.verification_id == first_request.verification_id + 1 + + stale = HeldObjectGuardResult( + verification_id=first_request.verification_id, + object_id="object", + attempt_generation=current_request.attempt_generation, + invocation_index=current_request.invocation_index, + next_waypoint_index=current_request.next_waypoint_index, + failure_mask=torch.tensor([False]), + state_invalidation=StateDelta(), + retry_mask=torch.tensor([False]), + ) + with pytest.raises(ValueError, match="verification_id"): + session.tick(initial, held_object_guard_result=stale) + + +def test_nonretry_held_object_loss_fails_row_while_peer_continues() -> None: + engine, _ = _engine(batch_size=2) + initial = _with_held_object(_context(0.0, (0.0, 0.0), (0.2, 0.2), 0)) + session = engine.start((_invocation(engine),), initial) + request = session.held_object_guard_request + assert request is not None + + partial = session.tick( + initial, + held_object_guard_result=_held_object_loss_result( + request, + failure_mask=torch.tensor([True, False]), + retry_mask=torch.tensor([False, False]), + ), + ) + + assert partial.status is ExecutionStatus.RUNNING + assert partial.eligible_mask.tolist() == [False, True] + assert partial.command is not None + assert partial.command.active_mask.tolist() == [False, True] + held = partial.task_state.get_held_object("arm") + assert held is not None and held.env_mask is not None + assert held.env_mask.tolist() == [False, True] + event_masks = { + event.kind: event.env_mask.tolist() + for event in partial.events + if event.kind + in { + ExecutionEventKind.HELD_OBJECT_LOST, + ExecutionEventKind.RECOVERY_REQUIRED, + } + } + assert event_masks == { + ExecutionEventKind.HELD_OBJECT_LOST: [True, False], + ExecutionEventKind.RECOVERY_REQUIRED: [True, False], + } + assert len(session.plan_attempts) == 1 + assert session.plan_attempts[0].action_retry_counts == (0, 0) + + +def test_missing_or_out_of_phase_held_object_guard_result_preserves_state() -> None: + engine, _ = _engine() + action = EffectAction() + engine.register(action) + initial = _with_held_object(_context(0.0, 0.0, 0.2, 0)) + base = _invocation(engine) + invocation = ActionInvocation( + skill_id=action.skill_id, + goal=base.goal, + binding=base.binding, + motion_policy=base.motion_policy, + recovery_policy=base.recovery_policy, + ) + session = engine.start((invocation,), initial) + + first = session.tick(initial) + assert first.task_state.get_held_object("arm") is not None + session.tick(_with_held_object(_context(0.1, 0.0, 0.2, 0))) + pending = session.tick(_with_held_object(_context(0.2, 0.2, 0.2, 0))) + + assert pending.pending_effect is not None + assert session.held_object_guard_request is None + preserved = session.tick(_with_held_object(_context(0.21, 0.2, 0.2, 0))) + held = preserved.task_state.get_held_object("arm") + assert held is not None and held.env_mask is not None + assert held.env_mask.tolist() == [True] + + def test_all_rows_planning_failure_skips_inactive_command_frames() -> None: engine, _ = _engine() action = FailedEffectAction() diff --git a/tests/sim/atomic_actions/test_runner.py b/tests/sim/atomic_actions/test_runner.py index 12bec40ac..c83216699 100644 --- a/tests/sim/atomic_actions/test_runner.py +++ b/tests/sim/atomic_actions/test_runner.py @@ -42,6 +42,7 @@ ExecutionEventKind, ExecutionRunner, ExecutionRunnerCfg, + HeldObjectGuardRequest, HeldObjectState, JOINT_POSITION_CAPABILITY, JointPositionPayload, @@ -57,6 +58,7 @@ RuntimeCommandFrame, RuntimeEndpointTarget, RunnerStatus, + RunnerStep, SceneSnapshot, SkillBindingContract, SkillEndpointRequirement, @@ -340,6 +342,8 @@ def _make_runner( max_action_retries: int = 2, action_timeout: float = 10.0, tracking_runtime: TrackingRuntime | None = None, + hold_on_completion: bool = True, + hold_during_effect_verification: bool = True, ) -> tuple[ ExecutionRunner, FakeClock, @@ -386,7 +390,11 @@ def _make_runner( provider, sink, clock=clock, - cfg=ExecutionRunnerCfg(minimum_cycle_time=MINIMUM_CYCLE_TIME), + cfg=ExecutionRunnerCfg( + minimum_cycle_time=MINIMUM_CYCLE_TIME, + hold_on_completion=hold_on_completion, + hold_during_effect_verification=hold_during_effect_verification, + ), ) return runner, clock, provider, sink, action @@ -411,6 +419,26 @@ def _successful_effect_result( ) +def _unresolved_effect_result( + context: PlanningContext, + request: EffectVerificationRequest, +) -> EffectVerificationResult: + """Keep every row pending at the current effect boundary.""" + return EffectVerificationResult( + verification_id=request.verification_id, + success_mask=torch.zeros( + context.batch_size, + dtype=torch.bool, + device=context.robot.qpos.device, + ), + failure_mask=torch.zeros( + context.batch_size, + dtype=torch.bool, + device=context.robot.qpos.device, + ), + ) + + def test_joint_feedback_ignores_motion_outside_bound_endpoint() -> None: runner, clock, provider, sink, action = _make_runner(control_joint_ids=(0,)) @@ -458,6 +486,56 @@ def test_runner_dispatches_only_when_timed_waypoint_is_due() -> None: assert third.wait_duration == pytest.approx(SECOND_INTERVAL) +def test_runner_calls_held_object_guard_with_fresh_command_phase() -> None: + runner, _, _, sink, _ = _make_runner() + observed: list[tuple[float, HeldObjectGuardRequest]] = [] + + def verifier( + context: PlanningContext, + request: HeldObjectGuardRequest, + ) -> None: + observed.append((context.robot.timestamp, request)) + return None + + first = runner.step(held_object_guard_verifier=verifier) + + assert first.status is RunnerStatus.RUNNING + assert len(sink.sent) == 1 + assert len(observed) == 1 + timestamp, request = observed[0] + assert timestamp == 0.0 + assert request.verification_id == 0 + assert request.segment_name == "timed" + assert request.attempt_generation == 0 + assert request.invocation_index == 0 + assert request.next_waypoint_index == 0 + + +def test_runner_guard_exception_performs_cancel_then_observed_hold() -> None: + runner, clock, _, sink, _ = _make_runner() + runner.step() + clock.advance(FIRST_INTERVAL) + + def verifier( + context: PlanningContext, + request: HeldObjectGuardRequest, + ) -> None: + del context, request + raise RuntimeError("guard evidence unavailable") + + failed = runner.step(held_object_guard_verifier=verifier) + + assert failed.status is RunnerStatus.FAILED + assert [dispatch.operation for dispatch in failed.dispatches] == [ + CommandOperation.CANCEL, + CommandOperation.HOLD, + ] + assert sink.cancel_count == 1 + assert [target.target_id for target in sink.cancelled[0]] == ["arm"] + assert failed.message is not None + assert "guard evidence unavailable" in failed.message + + def test_runner_dispatches_transport_neutral_endpoint_frames() -> None: runner, _, _, sink, _ = _make_runner() @@ -858,6 +936,62 @@ def test_blocking_runner_resumes_a_stored_effect_verification_boundary() -> None assert completed.tick.task_state.get_held_object("arm") is not None +def test_runner_holds_while_effect_verification_is_pending_by_default() -> None: + runner, _, _, sink, _ = _make_runner(with_effect=True) + + blocked = runner.run_until_blocked() + + assert blocked.status is RunnerStatus.RUNNING + assert blocked.tick is not None and blocked.tick.pending_effect is not None + assert [item.operation for item in blocked.dispatches] == [CommandOperation.HOLD] + assert len(sink.held) == 1 + assert [target.target_id for target in sink.held[0][0]] == ["arm"] + + +def test_runner_skips_all_effect_pending_holds_when_disabled() -> None: + runner, clock, _, sink, _ = _make_runner( + with_effect=True, + hold_on_completion=False, + hold_during_effect_verification=False, + ) + + blocked = runner.run_until_blocked() + assert blocked.tick is not None and blocked.tick.pending_effect is not None + assert blocked.dispatches == () + + polls: list[RunnerStep] = [] + for _ in range(2): + clock.advance(MINIMUM_CYCLE_TIME) + polls.append(runner.step(effect_verifier=_unresolved_effect_result)) + + assert all(step.status is RunnerStatus.RUNNING for step in polls) + assert all( + step.tick is not None and step.tick.pending_effect is not None for step in polls + ) + assert all(step.dispatches == () for step in polls) + assert sink.held == [] + + +def test_effect_success_adds_no_hold_when_pending_and_completion_holds_are_disabled() -> ( + None +): + runner, clock, _, sink, _ = _make_runner( + with_effect=True, + hold_on_completion=False, + hold_during_effect_verification=False, + ) + blocked = runner.run_until_blocked() + assert blocked.tick is not None and blocked.tick.pending_effect is not None + clock.advance(MINIMUM_CYCLE_TIME) + + completed = runner.step(effect_verifier=_successful_effect_result) + + assert completed.status is RunnerStatus.COMPLETED + assert completed.tick is not None and completed.tick.pending_effect is None + assert completed.dispatches == () + assert sink.held == [] + + def test_resumed_effect_verifier_uses_a_fresh_observation() -> None: runner, clock, _, _, _ = _make_runner(with_effect=True) blocked = runner.run_until_blocked() @@ -1126,6 +1260,34 @@ def test_runner_effect_timeout_exhaustion_cancels_and_holds() -> None: assert sink.cancel_count == 1 +def test_effect_timeout_still_cancels_and_holds_when_pending_holds_are_disabled() -> ( + None +): + runner, clock, _, sink, _ = _make_runner( + with_effect=True, + max_action_retries=0, + action_timeout=2.0, + hold_on_completion=False, + hold_during_effect_verification=False, + ) + blocked = runner.run_until_blocked() + assert blocked.tick is not None and blocked.tick.pending_effect is not None + assert sink.held == [] + request = blocked.tick.pending_effect + clock.advance(request.deadline - clock.now() + MINIMUM_CYCLE_TIME) + + failed = runner.step() + + assert failed.status is RunnerStatus.FAILED + assert [item.operation for item in failed.dispatches] == [ + CommandOperation.CANCEL, + CommandOperation.HOLD, + ] + assert sink.cancel_count == 1 + assert len(sink.held) == 1 + assert [target.target_id for target in sink.held[0][0]] == ["arm"] + + def test_runner_deactivation_refreshes_cached_effect_request() -> None: runner, _, _, _, _ = _make_runner(with_effect=True, batch_size=2) blocked = runner.run_until_blocked() diff --git a/tests/sim/skills/test_compiler.py b/tests/sim/skills/test_compiler.py index fdac46a71..c6328310d 100644 --- a/tests/sim/skills/test_compiler.py +++ b/tests/sim/skills/test_compiler.py @@ -70,6 +70,7 @@ GroundedSemanticCall, HandOverPoseProvider, HandOverPoseTargets, + HeldObjectGuardBaseline, RegisteredSemanticLowerer, RelationTargetGrounder, SemanticLowering, @@ -751,6 +752,17 @@ def test_pick_effect_spec_binds_destination_and_fresh_monitor_per_grounding() -> assert revised.effect_spec is not None assert revised.effect_spec.invocation_revision == 1 assert revised.effect_monitor.spec.invocation_revision == 1 + assert len(first.effect_guards) == 1 + guard = first.effect_guards[0] + assert guard.guard_id == "destination_attached" + assert guard.active_segments == ("lift",) + assert guard.baseline is HeldObjectGuardBaseline.PLANNED_EFFECT + assert guard.task_state_key == "manipulator" + assert guard.invalidation_task_state_keys == ("manipulator",) + assert guard.retry_action is True + assert guard.effect_monitor is not first.effect_monitor + assert guard.effect_spec.effect_kind is SemanticEffectKind.ATTACH + assert repeated.effect_guards[0].effect_monitor is not guard.effect_monitor def test_place_effect_spec_binds_source_and_verified_detach_baseline() -> None: @@ -802,6 +814,20 @@ def test_place_effect_spec_binds_source_and_verified_detach_baseline() -> None: ) assert isinstance(constraint, BinaryEffectClause) assert constraint.expected is False + assert len(grounded.effect_guards) == 1 + guard = grounded.effect_guards[0] + assert guard.guard_id == "source_attached" + assert guard.active_segments == ("approach",) + assert guard.baseline is HeldObjectGuardBaseline.VERIFIED_TASK_STATE + assert guard.task_state_key == "manipulator" + assert guard.invalidation_task_state_keys == ("manipulator",) + assert guard.retry_action is False + guard_pose, guard_constraint = guard.effect_spec.clauses + assert isinstance(guard_pose, PoseRelationClause) + assert guard_pose.expectation is PoseRelationExpectation.MATCHED + assert guard_pose.baseline_object_to_endpoint is None + assert isinstance(guard_constraint, BinaryEffectClause) + assert guard_constraint.expected is True def test_handover_effect_spec_binds_source_and_destination_relations() -> None: @@ -876,6 +902,26 @@ def test_handover_effect_spec_binds_source_and_destination_relations() -> None: assert destination_pose.baseline_object_to_endpoint is None assert isinstance(destination_constraint, BinaryEffectClause) assert destination_constraint.expected is True + assert tuple(guard.guard_id for guard in grounded.effect_guards) == ( + "source_attached", + "destination_attached", + ) + source_guard, destination_guard = grounded.effect_guards + assert source_guard.active_segments == ( + "transfer", + "approach", + "close", + "hold", + ) + assert source_guard.baseline is HeldObjectGuardBaseline.VERIFIED_TASK_STATE + assert source_guard.task_state_key == "left" + assert source_guard.invalidation_task_state_keys == ("left",) + assert source_guard.retry_action is False + assert destination_guard.active_segments == ("release", "deliver") + assert destination_guard.baseline is HeldObjectGuardBaseline.PLANNED_EFFECT + assert destination_guard.task_state_key == "right" + assert destination_guard.invalidation_task_state_keys == ("left", "right") + assert destination_guard.retry_action is False def test_registered_call_without_monitor_has_no_effect_contract() -> None: diff --git a/tests/sim/skills/test_runtime.py b/tests/sim/skills/test_runtime.py index 128f23d13..147b78ddb 100644 --- a/tests/sim/skills/test_runtime.py +++ b/tests/sim/skills/test_runtime.py @@ -32,6 +32,7 @@ ActionInvocation, ActionOptions, ActionPlan, + Affordance, ArticulationJointState, AtomicAction, AtomicActionEngine, @@ -41,8 +42,11 @@ EndpointBinding, EndpointTrackingChannelBinding, EndpointTrackingFeedbackAddress, + HeldObjectGuardRequest, + HeldObjectState, JointPositionTarget, MotionPolicy, + ObjectSemantics, PlanningContext, RecoveryPolicy, ResolvedActionRequest, @@ -57,14 +61,22 @@ ) from embodichain.lab.sim.atomic_actions.tracking import TrackingPolicy from embodichain.lab.sim.skills.calls import RegisteredSemanticCall -from embodichain.lab.sim.skills.compiler import SemanticSkillCompiler +from embodichain.lab.sim.skills.compiler import ( + GroundedHeldObjectGuard, + HeldObjectGuardBaseline, + SemanticSkillCompiler, +) from embodichain.lab.sim.skills.effects import ( ArticulationJointStateExpectation, + BinaryEffectClause, + BinaryEvidenceKind, ControlPartEvidenceAddress, EffectEvidenceBatch, EffectEvidenceSourceRef, EffectMonitor, EffectMonitorDecision, + HeldObjectRelation, + HeldObjectStateExpectation, JOINT_STATE_EFFECT_CHANNEL, JointStateEffectClause, SemanticEffectKind, @@ -602,6 +614,109 @@ def test_nonblocking_step_routes_effect_feedback_through_collector() -> None: assert system.compiler.monitors[0].requests[0].verification_id == 0 +def test_in_flight_guard_collects_live_evidence_and_builds_loss_reconciliation() -> ( + None +): + system = _system((EffectMonitorDecision(_mask(True, True), _mask(False, False)),)) + semantics = ObjectSemantics( + affordance=Affordance(), + geometry={}, + label="object", + entity_id="cube", + ) + poses = torch.eye(4).repeat(BATCH_SIZE, 1, 1) + held = HeldObjectState( + semantics=semantics, + object_to_eef=poses, + grasp_xpos=poses, + env_mask=_mask(True, True), + ) + task_state = TaskState( + batch_size=BATCH_SIZE, + device="cpu", + held_objects={"arm": held}, + ) + expectation = HeldObjectStateExpectation( + expectation_id="source", + relation=HeldObjectRelation.ATTACHED, + object_id="cube", + slot_id="primary", + resource_id="arm", + task_state_key="arm", + ) + spec = SemanticEffectSpec( + semantic_id="carry", + effect_kind=SemanticEffectKind.ATTACH, + skill_id="carry", + invocation_id="workflow:0", + invocation_revision=0, + env_ids=torch.arange(BATCH_SIZE, dtype=torch.long), + state_expectations=(expectation,), + clauses=( + BinaryEffectClause( + clause_id="source.constraint", + expectation_id="source", + source=EffectEvidenceSourceRef( + "test.provider", + "1", + ControlPartEvidenceAddress("hand", "constraint"), + ), + evidence_kind=BinaryEvidenceKind.CONSTRAINT, + expected=True, + ), + ), + ) + monitor = _DecisionMonitor( + spec, + EffectMonitorDecision(_mask(False, True), _mask(True, False)), + ) + guard = GroundedHeldObjectGuard( + guard_id="source_attached", + active_segments=("carry",), + baseline=HeldObjectGuardBaseline.VERIFIED_TASK_STATE, + effect_spec=spec, + effect_monitor=monitor, + invalidation_task_state_keys=("arm",), + retry_action=False, + ) + system.runtime._grounded = SimpleNamespace( + analyzed=SimpleNamespace(effect_monitor_ref=None), + effect_guards=(guard,), + ) + system.runtime._runner = SimpleNamespace( + session=SimpleNamespace(task_state=task_state) + ) + system.runtime._current_call_index = 0 + context = system.observation.observe(task_state) + request = HeldObjectGuardRequest( + verification_id=0, + skill_id="carry", + invocation_id="workflow:0", + invocation_revision=0, + invocation_index=0, + attempt_generation=0, + next_waypoint_index=1, + segment_name="carry", + env_mask=_mask(True, True), + allowed_held_object_relations=(("arm", "cube"),), + allowed_coordinated_held_object_relations=(), + deadline=10.0, + ) + + result = system.runtime._held_object_guard_verifier(context, request) + + assert result is not None + assert torch.equal(result.failure_mask, _mask(True, False)) + assert torch.equal(result.retry_mask, _mask(False, False)) + assert result.state_invalidation.held_object_updates == {"arm": None} + assert len(system.runtime._effect_traces) == 1 + trace = system.runtime._effect_traces[0] + assert trace.boundary_kind == "in_flight_guard" + assert trace.guard_id == "source_attached" + assert trace.segment_name == "carry" + assert torch.equal(system.collector.calls[0][2], torch.tensor([0, 1])) + + def test_result_metadata_is_json_safe_and_contains_typed_runtime_trace() -> None: system = _system((EffectMonitorDecision(_mask(True, True), _mask(False, False)),)) @@ -645,6 +760,7 @@ def test_result_metadata_is_json_safe_and_contains_typed_runtime_trace() -> None assert "feedback_mode" not in attempt assert result.calls[0].resolved_core_policy.preset_id == "runtime_test_preset" effect = call["effects"][0] + assert effect["boundary"] == {"kind": "terminal"} assert effect["effect_spec"]["semantic_id"] == "test.metadata" assert effect["monitor"]["monitor_id"].endswith("._DecisionMonitor") assert effect["evidence"] == {} diff --git a/tests/sim/solvers/test_base_solver.py b/tests/sim/solvers/test_base_solver.py new file mode 100644 index 000000000..f2192b27f --- /dev/null +++ b/tests/sim/solvers/test_base_solver.py @@ -0,0 +1,155 @@ +# ---------------------------------------------------------------------------- +# 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 numpy as np +import pytest + +from embodichain.lab.sim.cfg import RobotCfg +from embodichain.lab.sim.solvers import DifferentialSolverCfg, OPWSolverCfg, URSolverCfg + +UR5_DH_PARAMETERS = { + "d1": 0.089159, + "a2": -0.425, + "a3": -0.39225, + "d4": 0.10915, + "d5": 0.09465, + "d6": 0.0823, +} + + +def assert_ur5_dh_parameters(cfg: URSolverCfg) -> None: + """Assert that a UR solver config contains the UR5 DH parameters.""" + for field_name, expected_value in UR5_DH_PARAMETERS.items(): + assert getattr(cfg, field_name) == pytest.approx(expected_value) + + +def make_ur5_robot_dict() -> dict: + """Return a minimal robot dictionary with a nested UR5 solver config.""" + return { + "control_parts": {"arm": [f"joint_{index}" for index in range(6)]}, + "solver_cfg": { + "arm": { + "class_type": "URSolver", + "ur_type": "ur5", + "root_link_name": "base", + "end_link_name": "tool0", + } + }, + } + + +def test_solver_cfg_from_dict_constructs_ur5_with_derived_dh_parameters(): + cfg = URSolverCfg.from_dict( + { + "class_type": "URSolver", + "ur_type": "ur5", + } + ) + + assert isinstance(cfg, URSolverCfg) + assert cfg.ur_type == "ur5" + assert_ur5_dh_parameters(cfg) + + +def test_solver_cfg_from_dict_runs_concrete_post_init_once(monkeypatch): + post_init_calls = 0 + original_post_init = URSolverCfg.__post_init__ + + def counted_post_init(cfg: URSolverCfg) -> None: + nonlocal post_init_calls + post_init_calls += 1 + original_post_init(cfg) + + monkeypatch.setattr(URSolverCfg, "__post_init__", counted_post_init) + + cfg = URSolverCfg.from_dict( + { + "class_type": "URSolver", + "ur_type": "ur5", + } + ) + + assert post_init_calls == 1 + assert_ur5_dh_parameters(cfg) + + +def test_robot_cfg_from_dict_constructs_nested_ur5_solver(): + cfg = RobotCfg.from_dict(make_ur5_robot_dict()) + + solver_cfg = cfg.solver_cfg["arm"] + assert isinstance(solver_cfg, URSolverCfg) + assert solver_cfg.root_link_name == "base" + assert solver_cfg.end_link_name == "tool0" + assert_ur5_dh_parameters(solver_cfg) + + +def test_robot_cfg_solver_to_dict_from_dict_roundtrip_preserves_derived_values(): + cfg = RobotCfg.from_dict(make_ur5_robot_dict()) + + restored_cfg = RobotCfg.from_dict(cfg.to_dict()) + + restored_solver_cfg = restored_cfg.solver_cfg["arm"] + assert isinstance(restored_solver_cfg, URSolverCfg) + assert restored_solver_cfg.ur_type == "ur5" + assert restored_solver_cfg.root_link_name == "base" + assert restored_solver_cfg.end_link_name == "tool0" + np.testing.assert_allclose(restored_solver_cfg.tcp, np.eye(4)) + assert_ur5_dh_parameters(restored_solver_cfg) + + +def test_solver_cfg_from_dict_applies_other_derived_config_logic(): + cfg = DifferentialSolverCfg.from_dict( + { + "class_type": "DifferentialSolver", + "ik_method": "dls", + } + ) + + assert isinstance(cfg, DifferentialSolverCfg) + assert cfg.ik_method == "dls" + assert cfg.ik_params == {"lambda_val": 0.01} + + +def test_solver_cfg_from_dict_preserves_unannotated_config_attributes(): + cfg = OPWSolverCfg.from_dict( + { + "class_type": "OPWSolver", + "a1": 1.25, + } + ) + + assert isinstance(cfg, OPWSolverCfg) + assert cfg.a1 == pytest.approx(1.25) + + +def test_solver_cfg_from_dict_ignores_unknown_fields(monkeypatch): + warnings = [] + monkeypatch.setattr( + "embodichain.lab.sim.solvers.base_solver.logger.log_warning", + warnings.append, + ) + + cfg = URSolverCfg.from_dict( + { + "class_type": "URSolver", + "ur_type": "ur5", + "unsupported_field": "ignored", + } + ) + + assert not hasattr(cfg, "unsupported_field") + assert warnings == ["Key 'unsupported_field' not found in URSolverCfg."]